久久久国产一区二区_国产精品av电影_日韩精品中文字幕一区二区三区_精品一区二区三区免费毛片爱

 找回密碼
 注冊會員

QQ登錄

只需一步,快速開始

搜索
查看: 24437|回復: 14

運動控制入門篇GCode_Interpreter

[復制鏈接]
1#
發表于 2014-5-10 09:39:53 | 只看該作者 |倒序瀏覽 |閱讀模式
本帖最后由 xmdesign 于 2014-5-10 09:42 編輯 5 F. @; ?& x1 Z

# s% X# p) u+ L  X# }首先聲明
" H  V9 `5 r7 U  t+ V" z我不是專家,業余搗鼓
. Q2 Z2 b5 i: w- f1 D/ J源代碼首先來自網絡, W* C/ ^9 Z7 W/ W
開發平臺Arduino及Maple
- ]2 V* M4 i4 \9 ahttp://www.arduino.cc/
8 h. e% B4 l. d3 w& Z# bhttp://leaflabs.com/devices/
' m6 ~5 V% O9 M" ?國內活躍社區
& W, V0 _! f# b7 Bhttp://www.geek-workshop.com/forum.php
6 l* x! J8 f- ]& Y& [, \3 C
  C7 N, k1 i  {; }& Y; ]竟然缺少積累,首先尋找最大的開放式開源平臺,這樣可以積累全球范圍的創客和DIY者的腦力活動經驗/ N$ C3 I2 R  x: }
來入門了解熟悉思路架構等
1 E) l3 l# k2 E0 F
5 a7 S9 l2 {% W我只搗鼓了下8位機Arduino 移植到32位機Maple 方面一點點事情,
3 O7 F( q+ M; L* g$ l許多功能還木完成,不過作為低檔次得應用可能還有可能
/ j9 |" x# R" A, L! t+ k5 T  o  d  t' s+ N$ Z# K
我自己也是個半桶水,再樂意玩的起碼自學能力還是要有點吧! q& C2 `% s6 R: E

3 @  W5 {8 J- J! V拒絕 所有的求
. x( \1 K1 F9 _& r! _! s2 F  m求人不如求自己 不然木玩
, b3 R4 m6 T/ g& x4 o: ^" F( X. I- {) e- E
高手繞道 謝謝!5 o( z( r0 P  }, q6 Z( n: W

5 k# H% k- E0 p# v7 J+ k6 ]7 q( z
回復

使用道具 舉報

2#
 樓主| 發表于 2014-5-10 09:48:37 | 只看該作者
本帖最后由 xmdesign 于 2014-5-10 09:52 編輯 % c% `2 `5 m; Z/ ^

! s; L/ E: U5 l! o- H6 e' aGCode_Interpreter是比較容易懂的,木那些寄存器等蝦米開始不容易懂的東東3 ~8 @$ U- i! T+ q2 @' J
貼代碼先0 B4 r* u( R8 d5 W, M
直接Maple的,某寶許多超便宜哈7 V) a' X, l1 L% r8 }1 H% x
暈,我怎么上不了RAR?" T$ H2 z. W+ F5 [( L8 X
想玩的留下 e妹吧% z) _- `/ P& I0 i
第一個妹麻煩傳第二個妹哈
% G1 o. {" C, @3 \我平常雜事多( X; [4 [0 k0 z! s" o  L
誰放網盤上再,麻煩開放下
3#
 樓主| 發表于 2014-5-10 09:52:53 | 只看該作者
// Arduino G-code Interpreter for Rep Rap
) p$ W" d7 O% C% K* h7 H2 e6 V// v1.0 by Mike Ellery - initial software (mellery@gmail.com)
' Y& S; m7 `0 P" o, h( n// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)+ u; u0 k4 W# c+ @
// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com)
. ?/ j8 c) G. j0 n( A// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
0 X3 O. [" y$ ~2 p0 Y: J
( B, r" V# r$ r7 |, q# Q// Arduino G-code Interpreter for Macro / Micro photography! G* K: F: Q7 r( |4 @* e$ F: j6 w
// v1.4 by Gene Cooper - modified and setup controls for macro / micro photography (gene@fourchambers.org). Z/ {/ r7 O0 E1 N
//modified by YaoHan China 2010.2.15
. f3 D; x4 b( Z) w4 d* J  h7 y* H//modified by JiWei China 2010.2.22 (jwdesign@163.com,QQ:75990175)0 @9 s3 ?) ], K: ^4 v$ p* p& j
#include <stdlib.h>2 Z4 U2 L6 b" O
#include <LiquidCrystal.h>7 B* W( m  c" f6 `
, _- s3 |( S: b, ^. y$ o
//啟動(高電平有效)/暫停(低電平有效)  默認高電平
/ b$ L+ a& y1 M( ^#define BUTTON_CTL 35% s3 j& A$ |$ ^% g) j# r
1 Y: h6 @" {) E. t3 I
//點動模式鍵(高電平有效)  默認低電平; P7 u0 d# m0 q$ B
#define BUTTON_SS 364 J' M. a! ~5 v) T0 b

! w$ w+ E4 `  o+ w  P//點動鍵
5 k1 B' A2 H7 d5 T4 {- d$ @#define BUTTON_MAN 15
& {# y  H; n7 e8 A( q( j2 D% @* n
5 J& W* A9 i# z" ]5 D+ U//電位器速度控制
; D! D3 o2 g" B) J# g#define SPEEN_CTL 16! q7 a1 \; _' S5 E
4 b7 x# V- U! u3 R6 E3 m- w
//手動調速使能) {6 L4 O$ k8 D
#define BUTTON_SP_EN 17
' {* Q9 }  B/ s* w* X' G; ]$ a! t% s! m5 {
//LCD 類型 1604或1602
7 ~2 X. y1 T3 a4 y' H) I; W1 O& H#define LCD_TYPE 1604. m0 P, }( N% p5 P9 ]2 F1 i4 e
//LCD 引腳定義2 G# D' s; ~) u
#define LCD_RS 23
0 I5 z) l- u- n7 o8 P: h3 C' w$ T1 i#define LCD_EN 24
( W5 n! ]0 y4 N#define LCD_D4 25
+ k4 P) m  w7 a) l. I& V/ B7 T. H#define LCD_D5 26' N7 y. b9 V! i$ u4 |& ?) d, u+ p
#define LCD_D6 27, b7 H: O! M* H
#define LCD_D7 28
! S$ g" ^- c/ x2 g6 z' D7 P6 ]2 V7 g* F# y* S' J
//命令字符串
6 Z  v  Q  D, L6 u9 Y#define COMMAND_SIZE 128
0 W+ h5 T, y, B0 h! g8 v$ X" Dchar word_old[COMMAND_SIZE];
2 r( F, u9 v1 U/ Cbyte serial_count=0;
$ @. j- [4 k. t, yint no_data = 0;2 n; d  A* _' S+ J. k0 ^( J
//LCD 引腳鏈接 配置
# J9 z1 Q, C. Q0 H' N% |) BLiquidCrystal lcd(LCD_RS,LCD_EN,LCD_D4,LCD_D5,LCD_D6,LCD_D7);
0 t+ K6 k8 E) L0 T; E" h9 W& Q//停止控制,高電平有效, f2 W7 V% T* F$ g0 e) \+ `
#define BUTTON_STOP 309 O6 y( x# Y4 O6 C, T, M: |8 x
int stop_flag=0;
2 U3 U+ p, f0 [  }/ x+ E6 |
5 M6 E8 U  J$ o/ E//暫停6 s( H+ [# r2 f+ r- k) H
void pause(){
5 s5 ]# \" n+ n2 ^- e  while(!digitalRead(BUTTON_CTL));
0 I8 E- `5 }: F* s# P}
/ s* N4 t! m% G; ^5 l+ X& O( o9 R7 w7 I' [
void stopper(){* T" ^: U& D1 \( g
  delayMicroseconds(10);0 }8 Q2 L  G) R, U8 @' N
  stop_flag = digitalRead(BUTTON_STOP);
) f# L6 ^$ v/ z- T  F$ N}
6 K4 e. Z, L+ R) `2 b, A7 h6 q/ G8 \3 D  `# q% c4 q; b6 U
void setup()
* A; L! ^; Z8 G* ?3 ?% H9 I, I/ n{
" g7 B5 I5 i, O. A  //show start" D9 [, e4 y. `* i  ^0 H2 [7 ]# E
  SerialUSB.println("start");( Q* O& e3 f2 ^. q# I& Y! F

+ g0 C9 P! W/ D% V  //啟動lcd
: i/ _' W( z. q( T/ Y    lcd.begin(16,4);3 ]6 m! L9 k' r! p9 @$ E" ^1 x# e
    lcd.setCursor(0, 0);) q- }8 L9 }5 u. k
    lcd.print("hello, world!");
# a+ i. c6 M  f+ D: N  g
5 ~" C: Z* b/ w6 n) j$ A( ^  //初始化控制引腳及引腳功能
0 u* W/ `3 \7 Y2 s& i5 Z+ E  pinMode(BUTTON_CTL,INPUT_PULLUP);
" D6 G- J; ?& m, \3 L  pinMode(BUTTON_SS,INPUT_PULLDOWN);0 e5 z/ B$ m; t4 [& s
  pinMode(BUTTON_STOP,INPUT_PULLDOWN);
% ]( {( |" `2 W  pinMode(BUTTON_MAN,INPUT_ANALOG);2 R1 R& K0 T4 m) Y/ D  ^
* o1 @( L; q# n& `* P) P4 D5 g
  //控制引腳的中斷
  b7 T+ l7 P" o; d  attachInterrupt(BUTTON_CTL,pause,FALLING);//暫停的中斷7 z0 D- ~3 f+ `, A3 {- }
  attachInterrupt(BUTTON_STOP,stopper,CHANGE);* Z, M! d) t4 k' H7 K2 P( E

1 x3 v* Z0 X* w- {  //other init
+ _7 e9 \8 @* `. u- x  init_process_string();! r8 ^; Y: k& L. b( C7 c1 A( \$ d9 s
  init_steppers();
( U4 V/ s! ]* ^! _4 c5 G; e1 [2 n: b( ^" Q  init_camera();
' F2 b( ~2 N0 g6 f- i* o% o# V0 e$ @$ ^8 {  B
}8 t1 ]( F2 `. a5 X: J: M: o

# U; _! |" Z% D) yvoid loop(), y0 `* c% @5 l. F. ]1 W
{
- k3 }4 z0 W  }6 L- {5 o  char c;
  h1 W2 F' e$ a# v: ]4 r( @# X/ t. K0 G
  //讀取輸入的字符) t) C, \& @% l
  if ((SerialUSB.available() > 0) && (!stop_flag))
6 Z- n3 b7 s. F. ]* B  {
) Z2 t% }, l4 w0 x    c = SerialUSB.read();' n) r# ]# N5 R! w) c
    no_data = 0;
3 J  B4 p/ g4 n6 w% U
4 O, d! f' A- y9 U4 r3 z$ r    //換行符代表一個命令的結束# A4 {" N4 ~( Z9 W2 i) d! |
    if (c != '\n')3 Q# N# i  }, G2 W
    {! W+ e" x% v8 d% i8 P
      word_old[serial_count] = c;  C& C. D, z: I1 y7 `% K
      serial_count++;
( u5 B5 g: j* ]; M" ~/ z6 `2 t0 M
    }
) E; z: q, x9 u' R" s' J5 c6 p# Q) j3 L  }
0 x, N; B" }2 B; S" D; W# c) R  //標記沒有數據輸入
8 V/ a& u1 O" d9 p( J/ w. w* e4 e  else
+ m% M% c% j6 G6 {1 _$ |  {1 m6 [- P1 `, S: E
    no_data++;
3 V; s9 s0 W+ D! \) S4 l. E    delayMicroseconds(100);
/ u6 U# Y% n  p5 u2 ^2 h  }( B! p+ D7 c; U

# e4 t! K) W8 M' }7 T  //if theres a pause or we got a real command, do it
) o' d  B. p; Q. f( b5 h- t  if ((serial_count && (c == '\n' || no_data > 100)) && (!stop_flag))
6 G# V7 @; N3 P  {
$ e( O. d/ K+ j6 s2 E
  q1 R1 e& k' s    //處理命令
4 N( r4 S* l& C. |    process_string(word_old, serial_count);
2 `7 d7 ?: @# K2 E+ `# Q( e7 X$ Q+ P2 U  S
    //清除命令1 I! g4 R! Y( |/ `$ I
    init_process_string();, O- g: \1 I& \! O
  }
- f) L. i6 g3 a4 n
+ V; a' ~; J% L- l( g  //如果沒有數據關閉電機
. X4 y  r0 h# q' K* \  if (no_data > 1000). k% A/ P  F$ e' \2 ^5 Z1 R& A
    disable_steppers();) y: W& G7 f; J7 n: a
7 @6 ]. a8 g0 E1 J
  //如果ss鍵按下進入點動模式
$ K% a) s; e! G; ~; A7 e  //if(digitalRead(BUTTON_SS)) ss();7 [7 L5 O" j) O, [4 L7 s/ E
  run_a();9 h2 M; [; Q1 \0 r# Y! I
}  X1 k% M8 K0 O0 c6 v
' \+ I  d- H1 e8 Q# x$ v8 b
//點動模式
, ?5 ]5 s3 r. m# \void ss(){
% u+ ~' S9 w3 O( i9 k- z1 ]0 D" I" ~. j) g" o* \  I
  delay(1);  {( y/ l5 o* P  k, s# ^2 A
  if(!digitalRead(BUTTON_SS))return;
" o: d( v) [  S0 T) s/ J  if(!digitalRead(BUTTON_SS))return;
8 ?: j% w( b4 p& J  //init_process_string();+ V$ V: I3 Z$ s3 B) `- @
  //init_steppers();
4 H) `& g1 u9 \1 ]6 m0 h2 l5 X6 d3 q: {  //init_camera();
1 k% J+ n. ~0 L) N) b3 I0 u7 P: k/ k0 Q
7 C$ g7 D$ ?# ~& S // SerialUSB.println("Step By Step Mode");) N1 }& }5 S, u
//#if (LCD_TYPE == 1604 )
/ J, D1 z! ^+ J+ N3 _5 H  //當LCD為1604時顯示要處理的命令7 r6 m+ V  F4 H+ f, z6 F
   lcd.setCursor(0, 0);3 n+ I+ T5 Z6 X, R$ Q9 \( V' o
   lcd.print("Step By Step Mode");
8 L; {# K# p% A+ U6 Y9 P1 n//#endif: R. [; ]6 n, B3 g6 U* A. K
  process_string("G1 F5000",8);* A  J) z+ r! f5 `) H
  process_string("G91",8);
: V1 q, c/ Y; u- w' r- b  //init_process_string();( E# S" k2 \$ i1 K. V8 Y9 \& A

) G4 t; ^6 S7 r( Q, h7 v* z  while(digitalRead(BUTTON_SS)){5 G' X1 c& k* [) W+ z  Z: f+ E
    int i=0;
* M$ C/ Z  t; u: ^" X, J  n/ w, A    i=analogRead(BUTTON_MAN)>>9;
) n+ P9 v) d6 E    //if (i==0){break;}- z9 Z9 S" Y" u  ^$ Q1 x
   //SerialUSB.println(i);/ t4 [) H; `$ s* ?8 d
   //delay(1000);   
" C9 ?( ~1 n& V4 o' F/ k! b$ X2 M* l0 O# I) s  o
   if(i==2){/ ]4 h; r1 N/ U+ F# B
      set_target(1000.0, 0.0, 0.0, 0.0);
( ~& e8 L& g+ s+ e- w* j/ i+ u, y      dda_move(getMaxSpeed());# F3 b( x7 H! f) F9 u
      if(stop_flag) return;- Q( C  \4 ~; i5 _/ o6 B1 k
      //process_string("X0.01",5);
4 q4 {/ E# |2 W      //init_process_string();: E" R: o( C+ Z; D3 Z. h. d
    } 8 ]) g" f0 p! i
    else / m% D2 G( @6 r6 ~2 V& |/ h
    if(i==5){0 C0 H1 W- f: R" I% \
      set_target(-1000.0, 0.0, 0.0, 0.0);
7 U! X5 k$ |7 n" k* g. X% P0 g      dda_move(getMaxSpeed());
, h* K" c% t  r& T0 Y/ D      if(stop_flag) return;
1 ~' n4 G" `* J& `      //process_string("X-0.01",6);
* m) c0 T8 x( Y5 S' I2 o      //init_process_string();
3 h3 Q+ `& k9 h" L& _    }
' B; r; H7 n" Y* Z9 j! P     $ l1 \/ l' s% p0 @% N
; d4 T2 \' f& |) u8 ]& h
  }
9 t4 S: x# @+ o  x" j" @
( H$ h2 h: B8 @& F  //init_steppers();0 _' a" _: |) d+ ~7 J* f+ z
  //init_camera();
; x  m; h* E; ]6 u   // SerialUSB.println("Return To Normal Mode");
5 [' r  d9 A. x4 w1 G     process_string("G1",8);2 H9 m; _4 ]! A$ ^! G7 ?
  // process_string("G91",8);* N$ I4 ]/ l4 _( i$ @7 [
    init_process_string();
8 C6 b8 ?5 ~4 S" i4 s/ Q. K4 S  z//#if (LCD_TYPE == 1604 )$ ?+ q4 V' s  c
  //當LCD為1604時顯示要處理的命令) t) G1 Z8 Q. h8 A; p8 C! t7 u
  // lcd.setCursor(0, 4);
  _8 u1 M2 S2 j6 t  O  // lcd.print("Return To Normal Mode");
+ h. D5 p, j! \. A' O9 \5 o//#endif% r- g" H0 t# J4 P
}1 [/ ?  H% D& h5 u

2 t4 d1 a6 d; x* Y' nvoid run_a()4 t0 t# M  U0 `2 T, m. e+ U+ r
{- I  Y1 @$ }$ U0 m: N0 i% [/ N
  //delay(1);
' V% a% l& T# ~( h4 p  //if(digitalRead(14)== HIGH)return;
/ x7 c+ Y: w1 S7 G  c9 |  //if(digitalRead(14)== HIGH)return;
( w. F/ L5 o' n9 R9 S  //process_string("G1 F2000",8);
6 P2 P* @9 E0 b; q //process_string("G92",8);: Z, u% C6 I' i$ a2 f+ u$ y% V0 |
process_string("G90",8);: b, \5 V( C- B
process_string("X120 F10",5);
/ ^( ?1 T: T3 ~$ ~  F process_string("G4P2000",8);
3 V' @) G1 N6 {7 I. G process_string("X10 F10",5);, W) z" R3 T& B# r  S0 J1 T! [& A
// process_string("M101",8);5 g) M6 G$ }, b; M* P
// process_string("X-50",5);
* B0 b# B1 G8 A9 ~3 f dda_move(getMaxSpeed());1 E2 f- Z( o) d
init_process_string();+ `, T+ P, s& C5 r' \
" A' X# V4 u: X! [
}4 c/ O" ~, x( D. X2 L
4#
 樓主| 發表于 2014-5-10 09:53:25 | 只看該作者
. Y! ]2 q9 B4 R0 o  C- c2 z. L
% [5 d3 P! p! |8 J3 a4 ~4 \
// 定義機器參數
3 I: @+ c; Q' j6 M: u#define X_STEPS_PER_INCH 400
1 [# w4 r. e4 r+ R#define X_STEPS_PER_MM   16.0" R1 ^, z- x$ |
#define X_MOTOR_STEPS    200& Q+ [2 x" H+ q% A% ^
8 Y/ @/ |6 v/ V# I0 A' A  P
#define Y_STEPS_PER_INCH 400.0
4 p. q4 `6 y0 q5 t( q) \  N7 i0 L#define Y_STEPS_PER_MM   16.0
0 q) |. c  g8 r3 d4 E" D#define Y_MOTOR_STEPS    2009 O. W$ [! V3 J8 B

. j& j+ i1 E/ |. T1 ?/ p1 X#define Z_STEPS_PER_INCH 400.0
( Y$ `7 B/ p0 p' V9 e3 y#define Z_STEPS_PER_MM   16.0) k: @) _9 z: U* j4 j
#define Z_MOTOR_STEPS    200
; s' J' M3 e2 J3 g6 q' V6 P
7 H# ~" g% E+ }: |#define U_STEPS_PER_INCH 400.0
# y1 Q+ j- y$ K. M* p#define U_STEPS_PER_MM   16.0
% {2 U& g2 f4 ]+ p#define U_MOTOR_STEPS    200
" y  z/ J# v+ _9 L, r! f( W6 C
+ e$ `" o' L7 R5 B+ t! Y2 d//最大進給率
! N+ L1 Q: h; `/ V8 D2 a#define FAST_XY_FEEDRATE 1500.0
0 u8 S/ C0 y7 Y( z9 P" m#define FAST_Z_FEEDRATE  1500.0# W9 i* H0 I; D! p* L1 |
#define FAST_U_FEEDRATE  1500.0
3 a3 I5 v7 X4 G- }1 v// Units in curve section# n1 }3 t( y; P9 _
#define CURVE_SECTION_INCHES 0.019685
5 p8 g5 b# s% X8 G/ @/ C#define CURVE_SECTION_MM 0.5! a% W- x  t2 A2 n  k7 s9 h+ x
7 S# U- `* N( x5 U; z

2 ?/ [, e1 Z" g# Z0 R1 p  |// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)8 t4 U1 h3 B& w
// RepRap opto endstops are *not* inverting./ f- B% f' X4 P; b6 h
#define SENSORS_INVERTING 1: [% {0 W  R& t# W
3 H/ [/ t3 R" g8 @( g6 X" _
/****************************************************************************************/ G- Y9 n2 I( l! c% N9 J/ h2 d
* digital i/o pin assignment; S) O# f( B9 M- E" `
*
% d: f. i3 y6 y3 F8 k8 ^' d * this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5: Y# l; V( [5 o5 z  ?" G& {
****************************************************************************************/
6 h! }4 O2 k/ L" v% {; d4 a( }. \3 `2 w5 G
//camera shutter and control pins
5 ^( W0 D3 L1 _& Q: L  i#define CAM_SHUTTER_PIN1 29/ U- \8 {: g& Y) K# N) x
#define CAM_SHUTTER_PIN2 30: c. U& U" G' |& C% P2 X
//#define CAM_AUX_PIN1 31 // analog 0
: D3 N3 H7 ~  t' i) f2 E& [+ Q# Q& w//#define CAM_AUX_PIN2 32 // analog 1& [# m) p  X5 I( i, a0 r* P
//#define CAM_AUX_PIN3 33 // analog 27 a% E5 q+ P& C
//#define CAM_AUX_PIN4 34 // analog 3
5 w2 }3 F$ C9 Y( y
# @4 D: a: E. b; [, {" I4 [2 A2 l// stepper driver pins' T9 y, ^& Z  d/ H3 p  E5 j/ J4 j" I
#define X_STEP_PIN 7
  U7 f$ G& [4 i' W#define X_DIR_PIN 8
4 V) y" @% E* Y8 X. T#define X_ENABLE_PIN 19
9 e: d9 a  I# D) n! W1 v7 Z# Z
#define Y_STEP_PIN 9
1 C  ^# z! Q2 u- T1 y% j' H3 C#define Y_DIR_PIN 10
( V2 ^4 A7 w+ h. e( I% x#define Y_ENABLE_PIN 19
: V. e( o, X- B/ V& v0 T! p' V5 y% A: e4 Y6 p, S. y
#define Z_STEP_PIN 11: V" v* ^# b4 z5 t9 E5 }& Z
#define Z_DIR_PIN 12
1 C. c+ @6 y& M7 ~4 ]#define Z_ENABLE_PIN 19
3 k& Q6 W6 i/ b; o3 `& y
1 d# @" ?; n- ?6 J7 B# y#define U_STEP_PIN 13, f# k2 }1 r! ?# @
#define U_DIR_PIN 14
8 `. q! l) p. Q#define U_ENABLE_PIN 19$ Z. n& N  m  ?) j3 I

& q/ a5 M1 X) d+ f% j// limits not used right now
( f  p* d- i' O3 c4 i9 l#define X_MIN_PIN 14( s( {! F* |1 e2 d+ k
#define X_MAX_PIN 14( x7 Q& `, ~  G7 m( @; t$ ^( N7 E
#define Y_MIN_PIN 14
" D7 |9 a  p# ~#define Y_MAX_PIN 14/ \. g, E8 V0 [, U3 K1 |! a
#define Z_MIN_PIN 14
7 J& {0 ?& Q' K$ J0 H! |! r/ Q4 A#define Z_MAX_PIN 14
6 F7 a- ^# ~+ \  K  C1 B#define U_MIN_PIN 14
- E  X; f* ~; z# n/ M5 _% q#define U_MAX_PIN 144 d6 W. R. C6 {" p, C
5#
 樓主| 發表于 2014-5-10 09:54:26 | 只看該作者
void init_camera()
9 h9 {7 B6 f8 J! v{! f: x0 }% ?: o% C; P( d$ I1 n
  pinMode(CAM_SHUTTER_PIN1, OUTPUT);
# S* b6 \$ y6 C, c# t9 w  pinMode(CAM_SHUTTER_PIN2, OUTPUT);1 L/ Z7 [9 [9 L3 a: x: ?
  //pinMode(CAM_AUX_PIN1, OUTPUT); // analog 08 A6 y! O" i, _! J8 m& r. G
// pinMode(CAM_AUX_PIN2, OUTPUT); // analog 1
' A( j- |; y: J: b0 F9 w$ o5 {: B // pinMode(CAM_AUX_PIN3, OUTPUT); // analog 2
: C* F" A( _& ` // pinMode(CAM_AUX_PIN4, OUTPUT); // analog 3
3 b# d8 q! O  e' I3 X}; M3 N9 _  }( [2 R) h

' P! g) g( d$ b0 Avoid camera_shutter1(), V! f/ B/ E1 }" P: g
{
! n( D, i9 \1 A/ Q4 ?" {- I  // fire the camera shutter via relay...1/4 sec hold time- N/ N: ?! Y( Z2 I( U: [
  digitalWrite(CAM_SHUTTER_PIN1, HIGH);
) m; K, z. S9 H$ a3 s$ M0 I  delay(250);
6 ]# S: N2 B( e7 s3 _. i  digitalWrite(CAM_SHUTTER_PIN1, LOW);
8 |. h+ V. q9 b
% S6 T* r: V, _7 S; X7 L: r' K; r3 U}
) p% Y, S+ k( }9 q, n% h5 E/ Y+ Q! K5 R# ~' T& Q0 E
void camera_shutter2()4 S- U: E) I) V, ^1 E" Q, ~& g
{# k* A- ]) `! j6 F: B0 y1 A
  // fire the camera shutter via relay...1/4 sec hold time
$ M* D9 H- @# R$ l  digitalWrite(CAM_SHUTTER_PIN2, HIGH);
4 z- t; o6 U  Z, K6 K: j' t  delay(250);+ ^/ _  x* X; ~
  digitalWrite(CAM_SHUTTER_PIN2, LOW);1 a6 [9 G; M( t1 a1 l% C. w& w
" A1 x" s& S: _! Y" {
}
' Y% f6 f; O/ ]) A5 m* M( o8 B/*
+ y+ ^" t( Q3 w' c1 ^- s( Svoid camera_aux1_on()
% f1 U9 V- M$ X. ]$ b# w" i0 L{
9 {% }" O. R/ T5 I2 j1 D  // turn aux relay 1 on
3 T1 J; m" l2 E( o  digitalWrite(CAM_AUX_PIN1, HIGH);
9 Q9 K& U) q" |, V' n}: r: X' P! Z3 `% o  x) A& W/ i
: T3 b( s" w% V
void camera_aux1_off()5 o: _" X$ K3 t' [5 @$ H
{1 V7 `6 B8 X" R( C
  // turn aux relay 1 off- q5 w% x0 b5 |* H3 w3 s1 l1 a
  digitalWrite(CAM_AUX_PIN1, LOW);9 O  ^' ~# y. u% W- A; H
}" z8 l; K8 e4 p3 T; ?

8 X. o* R7 g. Z" b% f0 N+ D7 Svoid camera_aux2_on()
% V) h5 }1 W# Y# w" d{' N# P7 D  w7 m" Z) R; w: W% a
  // turn aux relay 2 on
' l' Q' w: b/ n  digitalWrite(CAM_AUX_PIN2, HIGH);
" w/ m; I: R1 }5 j* T5 v/ [}+ x3 \! K  |$ C+ I0 H

7 C! o7 G6 p4 ?$ N/ A; a" Lvoid camera_aux2_off()
) C1 U+ A+ Q& s{
2 d+ b2 L* `' }3 n  // turn aux relay 2 off, r* m; }) r% W" d4 [  s
  digitalWrite(CAM_AUX_PIN2, LOW);, J* t) U9 @6 m$ ~, m3 X2 U
}" K6 B  p) Q/ H; h7 A

* e3 M! d- d) R# nvoid camera_aux3_on()$ _" v7 R! N. ]! l$ `6 _- g7 @$ l
{. m) V; M+ Z  s6 Y5 w: ?' P
  // turn aux relay 3 on
) {8 C* s# A& h* p5 ]  digitalWrite(CAM_AUX_PIN3, HIGH);  L; c) p$ e$ q  G( \
}( e  |. X. q, Y- b, R

$ h* T# ~$ o& l1 [  }void camera_aux3_off()
0 ]" Y/ T2 d" E6 H+ |! j{
! y( o( a5 c9 c9 }9 @5 b5 h" F  // turn aux relay 3 off
1 ?) e9 k3 ?- |% g" S  digitalWrite(CAM_AUX_PIN3, LOW);' W6 {0 o, ~. |% m: C' f. @" d
}
1 a* {$ e( L, n7 t. O: g) K
  ^3 ]; m2 v9 @1 N% v7 h6 Kvoid camera_aux4_on()% n. A& n1 ~& M: y' X! C7 `
{
) R- l, ~1 O( C+ i  // turn aux relay 4 on
. ~; g8 `) s5 q9 @; \, S9 Z) N  digitalWrite(CAM_AUX_PIN4, HIGH);( b: T9 U( u4 D6 S
}1 w% i  q8 R: m1 G: X* S. K
% @: w, X; F, j& }& h: x0 h
void camera_aux4_off()
4 V3 ]& u7 d7 \" k  r5 i{
$ ?/ e4 }* Z1 y$ _: `5 E  // turn aux relay 4 off
# v/ }- [3 a# I$ M  digitalWrite(CAM_AUX_PIN4, LOW);
, Y/ Y$ [+ \+ A( C9 T2 C}
6#
發表于 2014-5-10 09:54:28 | 只看該作者
樓主推薦的網址不錯
7 r7 k: `; [* Z1 F! [) M; W1 ]請問樓主是玩什么的,用樂高玩具嗎?
7#
 樓主| 發表于 2014-5-10 09:55:02 | 只看該作者
// our point structure to make things nice.+ ^  b7 _- Q; f& \1 H
struct LongPoint {$ O5 U6 ]# i; u* x2 ^. r
  long x;* r: }3 Q& T) ^' o
  long y;' [& }5 z0 Y% F* D' H# E/ j
  long z;& e3 A: f0 B* Y, c* d
  long u;
9 S: \+ q) u- E" R( P};
/ e. a; i( q0 s. ~6 k$ O4 Z2 ^, D; l! Q: @- v# I5 v
struct FloatPoint {
6 S% B; h$ t8 E  float x;$ X% H0 w& Q; Q
  float y;
/ \4 a  J$ ^! }8 s/ Q7 n( U! w$ b  float z;/ Y: z7 E) K1 W, M8 d7 y
  float u;
, n5 `" ~) P8 N3 s7 P6 z, O3 T  t};, b: M9 O) d5 S

6 x! U  e: C3 T( I; d4 ^1 d$ yFloatPoint current_units;. v; @$ H6 ^9 d2 B  T
FloatPoint target_units;8 D, P  U, L& d$ R
FloatPoint delta_units;
2 J5 W4 B7 E7 |2 V! Z5 g/ i2 a2 j) ]1 T# e/ K
FloatPoint current_steps;5 J0 N9 V) g7 p. k/ D8 @! H
FloatPoint target_steps;
/ K: b: }7 W( I8 O; ^. x3 J1 F: zFloatPoint delta_steps;2 X! W( n: k. h. Z3 N9 U5 c

" m+ e# |% r/ m6 mboolean abs_mode = false;   //0 = 增量位置模式; 1 = 絕對位置模式
. N& d- x' I* a# i7 @0 `, S" Z! T. {0 {( x
//default to inches for units  _4 E  D- Y  O4 A% c$ h, |+ s# ~
float x_units = X_STEPS_PER_INCH;. e. c7 T; b! [% |1 R
float y_units = Y_STEPS_PER_INCH;: c' ~! \; l6 t5 z8 T: ?
float z_units = Z_STEPS_PER_INCH;' P; o0 d% m5 \# N. K1 @
float u_units = U_STEPS_PER_INCH;% h% Q7 z) R. T, k
float curve_section = CURVE_SECTION_INCHES;
: a8 |0 \( \% Y: z. P0 s5 x9 G) t* k% ?  G. ]
//our direction vars
& V) W. `$ ?- r8 I9 y$ ^1 ^6 Gbyte x_direction = 1;
8 Q1 \3 m- ]# A9 E+ Gbyte y_direction = 1;
8 ~& }3 P; I6 U8 _byte z_direction = 1;
+ G0 L# A% j; A# y; a8 U% u6 Tbyte u_direction = 1;/ W4 M0 F9 w$ T! C4 x6 [
6 Z  G$ A# J3 U* F4 [
//初始化字符串處理
1 j  l( B  Y* X) V" r, p6 Vvoid init_process_string()
$ ~7 p5 k& {( ?9 L  `% j6 A{
# q8 k) p" G( y/ u  P4 O  //init our command
1 I  q& I, `& u* L# O6 C2 ~  for (byte i=0; i<COMMAND_SIZE; i++)
5 z: G. R' P2 R. n0 O4 @8 ^    word_old[i] = 0;
0 z1 ]" Z4 S% E) r& s' Z  serial_count = 0;
* D' {4 y& \$ ^* S, u' D}
5 q7 A  u, H; H" P8 V! P! _  {$ P1 o6 H% H$ e- @
//our feedrate variables.
% ?" a+ E! C! B% l9 t$ C0 {. afloat feedrate = 0.0;5 O0 v# j+ C' M  n0 q
long feedrate_micros = 0;
- M  n7 N$ j# i( X6 H
: m2 o% A5 }% y+ s8 j0 |//讀取并執行命令
' [9 x. S/ ]( ]% I: O! T% u5 ]void process_string(char instruction[], int size)
  L" q4 {" v. c9 n& T: v: S{
5 Q" u( \. M" e2 i( D  //the character / means delete block... used for comments and stuff.
8 l$ {7 _" W* G4 O, R  if (instruction[0] == '/')3 R) k( L) y2 q
  {* x5 _& R  i5 w( Y3 y' k4 i; Q
    // SerialUSB.print("ok");+ o1 ]) }" N8 u, R% A
    // SerialUSB.print(byte(78));
* h1 d2 P5 E8 {    return;0 v( U) t4 C' y  ]& S1 G
  }
$ o, ]  y0 I3 [* L# p$ k  //init baby!/ d1 {  t0 }0 E  j+ B
  FloatPoint fp;0 g/ @" e+ B3 D8 j, m9 t1 h0 X. N
  fp.x = 0.0;
& I6 U/ O9 B# E* w  fp.y = 0.0;
- V+ n% }9 }, X7 C4 S  x1 @6 w  fp.z = 0.0;3 |3 z/ U4 M% P% V! B/ L1 K
  fp.u = 0.0;. m' X) t" O: X" o" u' X
. R$ H$ A, \8 g
  byte code = 0;
$ V2 |) l" H4 S  b7 T( o6 Z5 X- c, P% O. h% d) U* n& O
  //顯示在處理的命令+ Y1 p+ d# `( i% h
#if (LCD_TYPE == 1604 )0 G- ?0 S' U* Q- i
  // lcd.setCursor(0, 4);
( `+ f& k5 @* k+ R  // lcd.print(word_old);
) ?- i# s9 q! S0 u+ L' i- x1 T#endif
# }% n  V' P- x" q1 P1 w  SerialUSB.println();1 p" R5 M! G4 [$ ]* g% G
  SerialUSB.print(instruction);( M8 x6 i$ O0 R4 I" m, D- `5 d) j
  SerialUSB.print("\t");5 l, {' @" X& J" f: k
" \% k1 R6 x1 a/ L
  //what line are we at?, w: \1 t9 G+ g5 Z0 K6 f
  //        long line = -1;
! u: [: n0 g* c$ w3 v# ~  //        if (has_command('N', instruction, size)): o7 [) n9 r: a+ R  Q+ [$ B  b
  //                line = (long)search_string('N', instruction, size);! ^5 k, V1 V0 S6 V

+ r1 q& k4 J: k  /*. h- W8 e  z1 m/ G. r' r
        Serial.print("line: ");
1 @( M$ F8 b; S7 B           Serial.println(line);& K8 z: e1 V4 e# E1 d
           Serial.println(instruction);
  Y( S8 r  J! L, {6 j   */
( L5 n* ], `3 P" n  //判斷是否讀取了個 G代碼?# D6 L7 z" ?7 C2 v: K. z
  if (
! f2 }* b: R6 |" ~    has_command('G', instruction, size) ||' T5 ^6 T6 r" G9 f; p
    has_command('X', instruction, size) ||
( m% }5 g4 W/ m% t8 K  n    has_command('Y', instruction, size) ||
2 T$ `. c8 R; i9 N: r9 P- R    has_command('Z', instruction, size) ||. R$ {6 X$ b  j  w- |" Y0 p/ \! N7 z
    has_command('U', instruction, size)
: Z+ z; M1 g2 R# G    )6 A. Z7 e$ F8 u1 @# ~$ |/ I3 `
  {
- Y( z) ^1 p% H; t* v& Q  d, U6 c    //which one?
6 g" ^9 ~4 y. {  J2 S    code = (int)search_string('G', instruction, size);7 C) Q3 O5 }6 O  `+ {3 ~/ J
    // Get co-ordinates if required by the code type given4 I/ G/ _! a4 n
    switch (code)
! m& s( s# @2 r( t: [' L    {
4 \2 A: Z! ~! m" |    case 0:: L* s6 M7 N; y* `6 ~) D2 b
    case 1:
0 S4 N9 }' ~1 v6 \  |/ b$ o8 b6 ]    case 2:3 B1 K; H) S6 ^1 A% y
    case 3:( K; _+ O; m( |* v2 H" P
      if(abs_mode)
! y: ?4 ?+ C' R  i, X% Y9 {% @) ~      {% V& O# G' R% z
        //we do it like this to save time. makes curves better.+ V7 M7 A  H8 B& d0 }! |+ [
        //eg. if only x and y are specified, we dont have to waste time looking up z.
7 ?1 M% s9 E- g- @        if (has_command('X', instruction, size))
# H# B0 |( H6 p' Y: A$ I5 `% r" E          fp.x = search_string('X', instruction, size);
% U! `. A; j8 Y$ g6 T; x' U        else
* x; Z/ F5 x3 z          fp.x = current_units.x;
7 d# ~: e) S, d. s7 l& z
# Q# P, u6 O4 i& ]0 s; Q* @        if (has_command('Y', instruction, size))
4 T- w$ m; q* \. v4 @( Q2 ~3 R          fp.y = search_string('Y', instruction, size);  |( k1 R) W+ l2 v
        else4 d( }& k0 h# h  u+ X
          fp.y = current_units.y;
1 |% [# V6 ?5 E2 P9 S
* J' T1 S& K; N, x3 c        if (has_command('Z', instruction, size))
# U6 b3 p2 v# d9 z6 ^          fp.z = search_string('Z', instruction, size);& I6 {" i  q) K- M: C
        else5 K0 U  J1 Q! [$ {/ a4 m
          fp.z = current_units.z;1 e: y1 z4 C% e+ C
         
4 B3 {- l( Y% ]! T, ]        if (has_command('U', instruction, size))2 a# {0 O' ^- Z" X5 b( d) H) M
          fp.u = search_string('U', instruction, size);
" n9 R* x0 {! O5 Q$ }  p        else( y7 H# U. m0 g
          fp.u = current_units.u;
- i. z" C" _  O3 W8 e. q: R      }- O" a" R* i, R2 n0 _9 _& L
      else
; Y# D$ p# ^6 T      {# X' S8 o4 R1 ^9 ^
        fp.x = search_string('X', instruction, size) + current_units.x;
- a3 j9 N( b7 [- S  ~6 J3 ^        fp.y = search_string('Y', instruction, size) + current_units.y;
0 E4 w6 ]7 g. b# `        fp.z = search_string('Z', instruction, size) + current_units.z;
+ D5 O3 F, E2 L  S& m        fp.u = search_string('U', instruction, size) + current_units.u;$ ]- k4 X4 ^6 W
      }6 {5 S, `  F4 @- I: n
      break;
3 f: ^# ?/ b  g) v1 T) u/ L( u    }% f! M, z1 m5 M" l- x
    //do something!( `8 H8 B- N! v/ s
    switch (code)/ K' y8 |* h! _) ?  R
    {
, x% F& i/ ?, C; x: n( D: Z2 x      //Rapid Positioning4 M3 I1 P0 y9 ~8 E  h
      //Linear Interpolation& k) l" l' P9 B" v) q
      //these are basically the same thing.' F) ?- i4 u' E5 ^
    case 0:- j4 o: d. y2 L/ X
    case 1:
5 O. J  Q  z1 Y      //set our target.
: X2 {, k7 p2 w& w4 I* a      set_target(fp.x, fp.y, fp.z, fp.u);- _. @& E  H& ^' Q" _, }- p( P" M
      //set_targeta( fp.a);# Z, d/ u% h1 r, E; ]& N3 e& x
      //do we have a set speed?' y, e/ X0 h6 A
      if (has_command('G', instruction, size))
' p8 P# Z% Z. B, a6 r0 h      {  S1 D1 r' D. D/ u
        //adjust if we have a specific feedrate.9 P/ ~5 `& M) e6 B9 ]
        if (code == 1)
! x; a5 r; L  y9 @% T        {6 r# g6 w: h( A
          //how fast do we move?
6 G3 w9 j9 J# S8 Z* b! F% g          feedrate = search_string('F', instruction, size);
) V" m$ o) Z) E$ L# C1 k          if (feedrate > 0)- ?1 N5 ?. C* f( Z5 ]; ~! g7 A1 r
            feedrate_micros = calculate_feedrate_delay(feedrate);1 F. ]7 f0 T  S- S
          //nope, no feedrate
& @2 n! Z' B+ V7 t) s$ a          else
% J, A4 w- [7 Z2 o5 A$ p/ ^            feedrate_micros = getMaxSpeed();* p8 _1 ^- [" F+ l' ^
        }  x( W1 p  S/ k: l
        //use our max for normal moves.& s4 n: B2 H6 r6 o
        else
, u' M& i8 ^; f          feedrate_micros = getMaxSpeed();
) e6 F' v1 o6 N% F      }1 A( `7 r3 w9 H- S
      //nope, just coordinates!: b* O4 H4 P. M1 i
      else1 _( k. o) m+ y# q" x9 m/ b) T( [& Z
      {  ], B0 v/ Y& i' x/ |
        //do we have a feedrate yet?- G6 X" l# f- a5 l4 T2 _  a0 J
        if (feedrate > 0)
% f6 T9 x6 d- m          feedrate_micros = calculate_feedrate_delay(feedrate);! u! t8 r* O6 U
        //nope, no feedrate
) H4 C- ^$ s6 J# r; c        else
. {6 s7 Y" Y. E9 F! S! F          feedrate_micros = getMaxSpeed();8 E/ I) O2 K( b, U  ?& a
      }% v0 H  m# T- \- R. [5 c- C6 e

$ H% B% S+ A+ k, Q: F" Q/ z      //finally move.& s0 }/ T: r" V
      dda_move(feedrate_micros);
) D2 Z4 i2 n5 b4 O9 Y1 h      if(stop_flag) return;
, z2 G  B+ c5 ^      break;
! l0 N% [4 f6 s# ?; z- V4 d- g, i# _, q8 U9 z
      //Clockwise arc% _8 Z. [( m  g( u) j
    case 2:+ K& v4 m& o6 S, F7 R$ r- ]
      //Counterclockwise arc0 h: B* O3 I0 b8 p% v; ^
    case 3:
: G& d* e4 J6 w! M      FloatPoint cent;
3 M6 g, p9 p; t4 z- ^      // Centre coordinates are always relative
1 ~/ H! H* {: K( q      cent.x = search_string('I', instruction, size) + current_units.x;
# ^1 ~! l- a3 L2 P      cent.y = search_string('J', instruction, size) + current_units.y;
. X: f: t" A; \+ q! N9 v* m      float angleA, angleB, angle, radius, length, aX, aY, bX, bY;, S9 `3 E/ i7 F1 ^  O
- S* l+ C$ A- n+ b" X" Z# I  \
      aX = (current_units.x - cent.x);* W. k2 c! K9 @& T6 t% U, q
      aY = (current_units.y - cent.y);
$ j" N1 n. N: {: P  T) ]; q: r      bX = (fp.x - cent.x);
- N7 n7 |0 e. W5 O7 ]      bY = (fp.y - cent.y);- H' m6 g+ [  m' K9 V8 U/ E

7 O6 j& t: P* C* g      if (code == 2) { // Clockwise
  m7 [  i: v4 {+ Q9 d; |        angleA = atan2(bY, bX);
  k3 n6 w& ^4 r) Y  r' `        angleB = atan2(aY, aX);
8 u$ @, }. k+ k8 p, c      } & W" I- b5 P2 G9 d8 i
      else { // Counterclockwise
  b# y* I6 o( m; l+ \% S        angleA = atan2(aY, aX);
9 t# }8 i: ?4 c4 B& c2 V        angleB = atan2(bY, bX);# W, Z! m! G: s, B
      }3 {6 v" k) n# r3 z
      // Make sure angleB is always greater than angleA; C( I! C, S, d, R0 D: y, y
      // and if not add 2PI so that it is (this also takes
6 z6 u* b( ^' t      // care of the special case of angleA == angleB,
4 |) j9 I- P& n) }/ i      // ie we want a complete circle)8 y# ]3 m# F& a+ a5 H" Z# m
      if (angleB <= angleA) angleB += 2 * M_PI;3 a0 C- o3 V) Q
      angle = angleB - angleA;
$ D4 H0 W3 a* ^' {! V8 a# \- @" o$ J) z) {" b* \. }1 s; m+ c3 S
      radius = sqrt(aX * aX + aY * aY);
# G" o) i6 I. A* I+ @  f2 N$ y      length = radius * angle;& x, C  j1 H; X9 c
      int steps, s, step;5 x3 v2 d$ `+ N8 X1 @: Y9 T! ?% C
      steps = (int) ceil(length / curve_section);, D: C0 J( k( f

9 E* e8 {* o+ [: M4 r! G      FloatPoint newPoint;
) X* V' F+ k% B      for (s = 1; s <= steps; s++) {4 F4 v& k' J6 u5 P7 x; K
        step = (code == 3) ? s : steps - s; // Work backwards for CW$ E4 v+ l  d4 B1 k% ~
        newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
5 t. p* i0 W% [/ P8 C* V1 s2 }        newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
; A3 J- T# b8 j) u& @0 Y! ^        set_target(newPoint.x, newPoint.y, fp.z, fp.u);* G; H7 }6 N; h; a; ~& G

" G* B! ^/ @/ _6 I        // Need to calculate rate for each section of curve$ E4 c. [( M1 ]6 `$ ^8 {( \
        if (feedrate > 0); y5 V6 @4 o/ t2 B( I8 ?
          feedrate_micros = calculate_feedrate_delay(feedrate);; S7 j2 O, o5 L
        else
1 b8 n) o. M" t. u          feedrate_micros = getMaxSpeed();/ X1 f3 ^0 {! F5 ]! T+ M
; E8 N$ I' \) R: [
        // Make step2 R2 s  w. d  n* Y
        dda_move(feedrate_micros);# I; \; T; ^4 @7 o/ e+ A( F$ `
        if(stop_flag) return;
2 A! s* q8 p; T; i$ d7 z& `/ e0 {      }
5 c3 c) X6 N4 b' m, l* B# r$ q% \9 D
      break;
' t6 L0 i8 E* V+ E" c2 Q5 p. J$ V3 z" F8 n2 e% P( ^
      //Dwell
; ~8 F1 ~- G( L5 k/ p2 _    case 4:
$ D4 ~( u  p- o! c7 [0 X      delay((int)search_string('P', instruction, size));- o$ U  @* u' t: }, V
      break;
9 M7 y/ h4 }0 r; I2 O( G0 a1 ^4 {. j7 D, E
      //Inches for Units
3 i' d0 h( u0 K# l1 _/ {: C9 Y    case 20:
$ }% p9 G. ^- a( Z) ?      x_units = X_STEPS_PER_INCH;+ C- w  r7 G, G- y) e
      y_units = Y_STEPS_PER_INCH;) R/ l& S4 s0 @
      z_units = Z_STEPS_PER_INCH;2 k) {* v. h; P. j9 B" x. V7 Z+ P
      u_units = U_STEPS_PER_INCH;# t3 X& U; G) m
      curve_section = CURVE_SECTION_INCHES;
0 N) N& }7 q. y. n  |9 @+ \: ~$ j      calculate_deltas();
% u& H' V! O" I* D' ]      break;
( L) V( q) v6 z& K# Z1 ^. q% @. I6 Y" P* {5 L* p
      //mm for Units: T2 H+ W3 ~3 j$ }. @
    case 21:
9 b! x3 q% b+ {* o3 [! x: x# m      x_units = X_STEPS_PER_MM;  O; l1 q- _3 S: C* F5 y
      y_units = Y_STEPS_PER_MM;5 c. U  H$ E- v
      z_units = Z_STEPS_PER_MM;: C* d6 y; G1 d  e6 B( E6 q
      u_units = U_STEPS_PER_MM;* d$ @3 H- U+ t$ ]. [/ L) v
      curve_section = CURVE_SECTION_MM;
5 Q7 z1 C3 F: R; i      calculate_deltas();
8 s: z+ y. V; w& t' f4 p      break;
$ N# ]- Y0 @1 E2 J, o7 J/ y) q; ?# l* H- d" |
      //go home.- O$ t) W" S! r! }
    case 28:
+ B9 ?9 j0 B  N0 Y1 N8 m      set_target(0.0, 0.0, 0.0, 0.0);0 ~) ~: P# z' l$ H# _4 _# V$ j9 G1 b
      dda_move(getMaxSpeed());% n  l9 f. l6 w9 ^% H
      if(stop_flag) return;
: R9 l. V! i& [5 G0 y1 V      break;! I' z( ^. c# Q; N, |

: }( p: I. }- v      //go home via an intermediate point.6 N) g/ g" }: N
    case 30:! r6 |" U* z% d% n6 q# I1 J* r
      fp.x = search_string('X', instruction, size);
0 ]: N8 ]. ?. J, h. k1 @  z( T5 e- H      fp.y = search_string('Y', instruction, size);! C: @8 }0 _  j2 u# X$ i
      fp.z = search_string('Z', instruction, size);! C" I. `, W" `# b. ]4 N- q
      fp.u = search_string('U', instruction, size);$ E! |+ N5 b$ ^( W
      //set our target.% \8 I5 \) F2 ?* `! W: T/ i  j" V
      if(abs_mode)0 s, T! [( H+ C8 z. @" v. J
      {
) Y0 v( a8 \( ]6 ]5 I4 @) y        if (!has_command('X', instruction, size))4 u/ S, v. F3 G, G% O* S
          fp.x = current_units.x;$ O, v9 W# m  N1 T) r% Z- b- \6 Y
        if (!has_command('Y', instruction, size))& ~: T' R8 |# S2 S
          fp.y = current_units.y;1 H: y# H, U1 Z/ P
        if (!has_command('Z', instruction, size))
  a* e! v* e( E% C          fp.z = current_units.z;+ ^  E0 V3 a1 Q  D8 E
        if (!has_command('U', instruction, size))
, ]$ c" N/ j! T; \% x          fp.u = current_units.u;
9 g+ f" p1 T- J        set_target(fp.x, fp.y, fp.z, fp.u);
& Z$ g* e& r/ O# L+ o: U0 B        / G2 b1 F5 T) E' c  y% R2 u
      }
# h5 A& i& i; s3 X# ^      else
7 Q7 g8 T9 l4 ~        set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );" P5 N) Z1 c6 ]! c; T
       8 d  A& e$ D7 k: ^2 L' ~
      //go there.5 }, C' b$ H: d9 U7 u; J0 r1 r
      dda_move(getMaxSpeed());, m/ Q1 Y8 s7 Q3 ~! z
      if(stop_flag) return;
2 B, i/ }2 i3 t$ N7 d* W% a
8 _4 g2 _6 a: z2 c      //go home.) w8 E& ~! V: y# q- `+ A# b
      set_target(0.0, 0.0, 0.0, 0.0 );! \2 Y$ }7 q# V) f* a
     
. O! s1 B8 z: W, y# v1 G% |* [      dda_move(getMaxSpeed());, M% [* `6 t* q0 h
      if(stop_flag) return;
. N" X* m& \5 h5 d1 I9 u* X      break;
, S9 [+ n! d* p9 Y6 V2 K' R, N8 O$ i: `- ~
      //Absolute Positioning  X/ k' b5 q" ?( a% \3 w4 }1 M
    case 90:2 b4 `9 |( `& c' ?2 [' X
      abs_mode = true;
* n; a7 }& P2 U" [' o6 j      break;
7 u5 `; u+ G9 p9 x% [3 X5 ?! ?* J  T1 ^4 v" x5 _: D$ b
      //Incremental Positioning0 G" h: p- l3 J2 Y! [
    case 91:
" ]5 c; J9 M/ ^      abs_mode = false;) z& P0 @$ h+ D9 ?9 G3 m; Y
      break;# X  i9 A1 ^: L, ?
) f5 X: D6 d  n4 x, Q2 V+ ^
      //Set as home0 P' c+ u% j3 W- e) K' n
    case 92:
) S( B% O; T* N' ~7 C     , D  |, t! M. }; i" U$ `+ j& \
      set_position(0.0, 0.0, 0.0, 0.0 );  |5 X) D+ D9 Z; m2 T& M
      
& `. O! i4 U, h# M. f+ Y" }+ u      break;4 s0 O7 k$ c3 a/ q/ A  Q
, G0 j. q# o& }: I' O- A$ M, t6 z
      /*
4 O- c. q' ^4 W3 a0 x0 W                        //Inverse Time Feed Mode2 |% g7 R, i$ j) ?% W- |
                               case 93:  N+ X# V) H3 [  T2 S
       5 i( k4 C& P( C9 b2 I" v& x- L
                               break;  //TODO: add this* f1 T& j% Q) ?9 P# ]( `8 b- ^
      
+ u5 t- y" |+ `! {- D0 }$ E                               //Feed per Minute Mode: w& u) H9 S1 `3 @* G9 I# q6 W8 ?
                               case 94:( S# {$ @2 p0 t9 _* m7 p& I9 {
      
; Y/ L( e# U  V$ ?' k5 d                               break;  //TODO: add this) R6 {5 z' M: F. z  d
       */
0 G# n& i0 P- A: G6 {) v# X% Z# C6 A. I- ~  e* r
    default:
1 v; u8 C; u" r+ c9 _0 \. }      SerialUSB.print("huh? G");0 N0 ?6 N3 l5 ?; n4 U) q" u& K# p
      SerialUSB.println(code,DEC);
% k8 K$ O0 ?  s: L- M    }
2 k( @; A/ c! [4 p  }
0 n; g+ ~( I5 W( k* }3 W( s( Z' [% k5 Q$ x6 ~, ^
  //find us an m code.
: k) K7 \* ~9 a! d% _  if (has_command('M', instruction, size))3 `1 h" b; h% l% r4 A, p/ ]
  {
+ |; N) G# m! ^$ I5 T0 C    code = search_string('M', instruction, size);4 t" l6 N4 p; u! l% v: j! [
    switch (code)- v( ^( J: ^0 B  L1 s% y
    {: s; q6 ~1 h) q9 x, W
      //TODO: this is a bug because search_string returns 0.  gotta fix that.' x5 M- b' x# B; `2 e. Y1 P
    case 0:6 u5 f- d' t- o0 }9 S
      true;: M, X- R- @3 r, t: e
      break;
$ b8 d4 p4 _# R1 S
6 f' r, f. D$ k, g% n    case 100:# m8 J+ I  p( `! B+ \- k6 U* G
      break;
' g( k/ \1 E+ o* H* q( h+ D5 u: z' ]5 x% g) P/ k) g% t
      // fire camera relay
* T% O+ x; j& M3 _    case 101:
" M6 |: [8 A" w, I      camera_shutter1();1 X$ J# E# c! B' g1 b8 t
      break;5 N0 }* W) `+ l# y7 Z  m9 u
) h3 X& ~; H. ~9 U; _3 }/ F7 s$ I
      // fire camera relay2
2 H1 N( G; }# P    case 102:' e3 ~; M6 D3 p& U" \. j
      camera_shutter2();
* w: Y# ?- q1 a5 q      break;) a: ]1 Q& A$ b9 N" s6 N
/*
1 q. v  `$ L. |  c5 U4 y      // turn aux 1 relay on
5 d& K; c1 g' K1 \! Q    case 103:
- c( D6 u8 d; j" y& `/ z: b6 F      camera_aux1_on();1 O, J" C9 [6 [0 @9 w9 F5 m( l
      break;
( N7 w) v2 _9 k; W$ g: q: p; N% s4 i8 P
      // turn aux 1 relay off9 d$ p! i# a% m5 e0 O# `
    case 104:
5 V6 R( f+ c9 V$ _7 \- S# Y* Q      camera_aux1_off();
1 ^- G6 x- |# B% }- S      break;
+ T5 ~. M& s9 C: ^) |* m5 n0 c: T, r9 d" s5 G+ W1 k# I
      // turn aux 2 relay on
. k  H9 w. e& \0 @) z4 ~    case 105:
) _3 K6 M3 s; S) @4 u$ B      camera_aux2_on();/ w( ~! v) V2 q0 Z( m$ S
      break;
2 U( b  n9 [7 i- D; ^8 P7 u  ^$ S( b, P' q# p! h& N: P4 a
      // turn aux 2 relay off
2 Y) |2 a" r- ^+ d    case 106:  h+ o9 @) S/ c1 H3 ?  _
      camera_aux2_off();' l4 }" y7 P0 Z) u
      break;& I, s& d6 f+ a( d6 x' u

, f' N" j9 ], h9 ]( j      // turn aux 3 relay on8 n2 A& v- I6 E& c
    case 107:
# h6 F, l1 `  G% Z9 g      camera_aux3_on();% K0 R) M5 U7 `) A: E
      break;
9 n' O' {1 k9 ]# P
6 Y# R1 ~% J5 Z' n5 q      // turn aux 3 relay off
* w* N: x# C1 [( m( U0 f0 K    case 108:0 R) O4 z' }, t6 [6 W) J- M
      camera_aux3_off();' L  }$ M1 \: M: S- g
      break;# O8 C7 j8 h7 u9 `
# Q0 _( f' j* z- J& t
      // turn aux 4 relay on# k9 a, L3 m* x8 U  c4 f' ~( j
    case 109:
  |& B/ W' A7 W9 L# n      camera_aux4_on();5 C, S6 f  @$ b3 `
      break;
/ ?+ D! G( p, U7 |4 m6 h1 g$ a) y8 w6 M3 f, Y8 }
      // turn aux 4 relay off
7 e8 [! u1 Q  X+ a    case 110:
% m% @9 h) T4 t9 Y) m+ O      camera_aux4_off();! X# f3 \; g- r4 B* p
      break;
9 M/ q% S% N1 E# j6 e*/
) U' W: y: ?/ J' r8 n5 B    default:
. S" h8 u" ]+ R- F" }
, e' V" [/ L* f      SerialUSB.print("Huh? M");
: i& D( n7 C3 z4 e$ B) H. R& Q      SerialUSB.println(code);8 o7 o8 [- ^# ?( s, }
    }; h: s# E. n! U
  }: j0 f( E( P. t" q- i0 i5 g" I
) h: ^* z* n$ \/ N6 {, ~! O
  //tell our host we're done.
. Q$ E+ t6 Z6 m% U  SerialUSB.print(byte(78));0 r; L% ]* ?' H* k0 ^
/ [' u& s. [. I3 a  y1 j% @
}9 Y7 y! d5 O1 ^7 ?! f  H$ X! y' h
: a/ t6 m% j* k
//look for the number that appears after the char key and return it. I) Q7 g; G4 ~* ]: u7 J$ E
double search_string(char key, char instruction[], int string_size)# V0 b: S5 v/ }- l
{+ ^7 w% C& n5 C/ D
  char temp[10] = "         ";
2 N7 b7 p: y  p/ E% ~& b  for (byte i=0; i<string_size; i++)
% w- o# \7 E$ H* K  {
. i- v4 ^& H" y0 a    if (instruction[i] == key)& ?- F( a2 {, i  m
    {
" Q+ n6 _4 [) t/ c4 u2 T1 G      i++;      5 Z  c* F) ?  f: y
      int k = 0;' ?+ H$ u6 V' V; E( b7 k7 ~- J7 B0 ?
      while (i < string_size && k < 10)
) J, T, r: U( G8 k+ t      {' W% d3 m9 w" }( g7 V
        if (instruction[i] == 0 || instruction[i] == ' ')9 z/ i0 m7 @: Q+ u
          break;
8 L7 g! O! Z8 n: W% A
2 }" i! q' x! x, u        temp[k] = instruction[i];
/ Z9 z& Z' m/ |+ T5 {, G( o        i++;
! w6 k2 ?: X6 m# I6 l: G/ p* }- K* h        k++;0 h  e, t  C6 F, ~8 ?# E
      }
3 {0 D, {9 n4 q7 J  R      return strtod(temp, NULL);# E: O3 \9 d( l
    }
( _4 w$ Z- {  K8 j+ O  }5 ?* v7 `5 `7 c6 k1 U

% o' T9 z& z7 J9 C, p' L: g  return 0;% ], u; B3 L- _# {1 }
}
& R5 U) d9 y; V4 j* k$ k
5 Q/ K% ?. k7 j3 P3 J( j" f2 A//look for the command if it exists.
6 Y$ p, H( y. x8 F+ ]; Ybool has_command(char key, char instruction[], int string_size)
6 }- z8 U3 W' a% T5 _0 t{
1 l8 T* q* `1 g% [* S8 r  for (byte i=0; i<string_size; i++)+ {6 B5 q4 [) c- P) Q0 m7 \
  {
; r  P2 T2 N8 a) |9 m    if (instruction[i] == key){" }: d% [- B& ]$ |6 z

5 {6 K1 L8 s' q      return true;
8 j( K: [( ?! }# p    }& Y( A  K5 Y( \+ _6 d
  }
7 f, z, \; }; ~. ~" _2 h5 G
: ]1 m7 z; x  f' ?! [  return false;. g" ]" Z5 B- f$ ^5 |4 C
}2 q' o1 r9 g: u2 M" I9 Y
, v+ k# j" u: E4 f3 p4 b
9 L, K. j) ]1 i3 b( g8 C. z+ b

0 n2 F7 `4 i: o, b/ h% ]6 M
8#
 樓主| 發表于 2014-5-10 09:55:37 | 只看該作者
/ o: m8 L. n4 j, t& B6 i
//init our variables
& `7 r. Z) a7 j! vlong max_delta;8 |! J4 z+ q( _2 c% C7 |- o6 j6 ]
long x_counter;
; S+ _4 U1 N' ~# elong y_counter;% J  W: W: d, k
long z_counter;
3 p7 B- D. k8 s) V* }0 Clong u_counter;
0 O& X# _8 @1 m2 C' y# zlong x_pos = 0; // x position in terms of absoloute motor stepps
7 q) N% T5 Z% `. K& S: u: Olong y_pos = 0; // y position in terms of absoloute motor stepps
( A0 A; s5 w& K$ l; F$ O% Mlong z_pos = 0; // z position in terms of absoloute motor stepps
+ O+ @, R$ Z3 u& h, J7 s) J. along u_pos = 0; // U position in terms of absoloute motor stepps
# p8 `& R; y7 y: m+ s2 G& l. g1 O. a& O/ \) i8 ~3 T
bool x_can_step;
- L/ \, w3 O2 d0 jbool y_can_step;' L6 }1 `# U+ k# X2 e
bool z_can_step;
$ Y0 B1 O! {" m, Z" dbool u_can_step;
( D4 w2 P  g4 r/ l$ |  \int milli_delay;
/ I9 _3 v1 j$ P: P3 I6 z) m  @$ I9 X1 z1 |7 e  g% C) E. X0 b
void init_steppers()
5 E8 [' m3 h2 }7 z; U{( B- D* s5 i; r  ~$ O2 g% v+ N7 e# v
  //turn them off to start.
" T( J8 R: X. e8 H6 ~  disable_steppers();
- {  R9 s& R7 r8 A' m7 P5 S9 f3 r6 A' Q4 Y% |9 y
  //init our points.
! o0 l0 i8 t1 i( H  current_units.x = 0.0;
% F! e$ |& N1 K1 Z5 p  current_units.y = 0.0;
4 ]. q# E3 _' [$ K. z% x  current_units.z = 0.0;
  V1 @  ?2 r" \  current_units.u = 0.0;! z0 S7 ~) J. k. w
  target_units.x = 0.0;
2 n5 `, }1 r* c4 W& l  target_units.y = 0.0;& o1 _$ Z) {' H3 T2 n- A
  target_units.z = 0.0;
5 g+ @2 g$ U* f  target_units.u = 0.0;- v3 @2 Y4 W, c3 A4 r0 S
  ; y( D5 o" l' B2 C

. G# r% B6 W# C/ P, p! i1 d  pinMode(X_STEP_PIN, OUTPUT);$ ^- w1 q  S, j) N2 m
  pinMode(X_DIR_PIN, OUTPUT);0 p% Z7 D7 v5 l( k/ B6 c
  pinMode(X_ENABLE_PIN, OUTPUT);
4 x2 o, p; e4 j5 J9 U0 ]  pinMode(X_MIN_PIN, INPUT);% ?1 X8 W* A8 ?' N: z
  pinMode(X_MAX_PIN, INPUT);
, f4 G' d0 P4 X/ I/ G- |1 ?1 V- A( u8 f
  pinMode(Y_STEP_PIN, OUTPUT);' ]3 S: q5 c$ P
  pinMode(Y_DIR_PIN, OUTPUT);& D: j, E; T/ O! W5 Z! D+ R& b- t
  pinMode(Y_ENABLE_PIN, OUTPUT);
( f3 ?: V- |+ _, l& u" P( ^# W  pinMode(Y_MIN_PIN, INPUT);( g1 ~5 L2 X7 f' B
  pinMode(Y_MAX_PIN, INPUT);' Y5 y! S8 p( F* M

) f$ n1 l! i1 g* L' _# |  pinMode(Z_STEP_PIN, OUTPUT);9 F. j' M/ p! Q$ W* _$ r) c
  pinMode(Z_DIR_PIN, OUTPUT);
5 m3 O$ y8 l. g  pinMode(Z_ENABLE_PIN, OUTPUT);  g% j$ M0 A7 P4 a- z* _
  pinMode(Z_MIN_PIN, INPUT);
! ]+ e& y7 e7 q$ x  pinMode(Z_MAX_PIN, INPUT);) }' t- G* A9 r, O; {+ C

7 m0 Q* [) F1 G# \1 x  pinMode(U_STEP_PIN, OUTPUT);4 E8 i1 Q4 t" b' q7 n
  pinMode(U_DIR_PIN, OUTPUT);
) `; [0 J9 c; k5 C' l  pinMode(U_ENABLE_PIN, OUTPUT);; r# y* M5 W) q( W
  pinMode(U_MIN_PIN, INPUT);1 ?7 ?' h) a3 b: S
  pinMode(U_MAX_PIN, INPUT);8 u! C, t5 K& @& g/ @$ ~4 T
  //figure our stuff.0 z6 @2 U' H; N( G/ E8 e* _' b$ _6 y
  calculate_deltas();1 C7 ^8 t  |5 q% I" b
}
1 ^% P  R- o- h5 A# ?3 h2 E% K3 f) s& R# t- f0 d  b" t
void dda_move(long micro_delay)
" q( j8 z# l4 [) K9 q6 e4 u% |3 ?1 g: X{
% i. Y, R. l- L# v; T, _+ r& q  //enable our steppers, f) x9 W6 b+ L7 W3 a0 [1 [# W+ L
  digitalWrite(X_ENABLE_PIN, HIGH);8 T( ^8 s9 k0 W2 m8 G" X% C
  digitalWrite(Y_ENABLE_PIN, HIGH);
$ i! d; n& p$ k& T  digitalWrite(Z_ENABLE_PIN, HIGH);0 y) T0 [) U3 `/ n. _
  digitalWrite(U_ENABLE_PIN, HIGH);
7 X; E# P; e5 V& F8 h  //figure out our deltas
1 o( y+ S  k) w( ^. ?0 A  max_delta = max(delta_steps.x, delta_steps.y);' k) p# j& O! g/ g, f& H+ |6 p
  max_delta = max(delta_steps.z, max_delta);
7 ~1 N4 p' g& A" b5 ]* J2 {  max_delta = max(delta_steps.u, max_delta);
7 f. p( A! |" n2 ~4 ^  //init stuff.$ @0 \5 U% U8 ~3 z) L: N( k& `* C
  long x_counter = -max_delta/2;1 `# R1 U# d7 p" m) d+ |- k3 e$ M
  long y_counter = -max_delta/2;
0 g: j% X2 Q: d9 B0 V* L1 L5 [  long z_counter = -max_delta/2;% q& N5 b! }, D3 U
  long u_counter = -max_delta/2;1 s0 m" L: h: T* }5 z/ |# \
  u, G- Z9 u1 _$ z' Y! ?
  //our step flags
8 Z6 N) i" U! g! e1 P  bool x_can_step = 0;8 [8 V4 l5 k5 j% O
  bool y_can_step = 0;
9 K" `1 K/ M& a& \  bool z_can_step = 0;- K" U( r5 V8 x! T$ v' U1 w- Y& ?* Q
  bool u_can_step = 0;4 J: Z4 e4 R- W2 M+ g

+ x: q% L. H# f' z# k; U  if (micro_delay >= 16383)0 W8 b+ G% ?/ ?  e
    milli_delay = micro_delay / 1000;3 i" Y1 |. K. H  z4 {
  else2 h" l' ~, J" C2 j* y
    milli_delay = 0;; A& ?, \+ Y4 R  \1 U7 |3 j. Z

8 }2 P8 x- f. O5 g$ L, |( U  h3 d# K2 \1 x
  //do our DDA line!% @# q3 ^3 V* }+ V

/ V( C  X. P: [  do: \% n; C5 g4 q% z& \
  {, e3 u, n& h& A! L# ?( {
    if(( digitalRead(BUTTON_SS)?analogRead(BUTTON_MAN)>>9==0:0) || stop_flag) break;9 u! x& s3 }; `" H/ ~' K9 G
    x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);  J( R  i) A' y& L/ }$ [
    y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);; n+ K( o5 p0 D9 B! C
    z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
8 |  K* k: @( d" Z8 |# b& y' O    u_can_step = can_step(U_MIN_PIN, U_MAX_PIN, current_steps.u, target_steps.u, u_direction);: ?4 M- q' a4 F" y( D0 {2 @5 I
    if (x_can_step)+ Y& `; a: ~4 S+ \
    {
7 d: q' U3 ]9 m2 K* R0 m      x_counter += delta_steps.x;
" S, Y* e/ q0 x% m! f; X" Z5 E; X1 J- ?5 p: H
      if (x_counter > 0)7 @! }* _9 U0 p' f
      {
- @$ _, m; R0 J. N1 \        do_step(X_STEP_PIN);- t* F8 ~& [2 K
        x_counter -= max_delta;
3 r; [) r; Y$ E3 S+ f4 e        if (x_direction)
, E5 w: D# @9 n. J  r3 C6 n+ z         { current_steps.x++; x_pos++; }% }! k0 R. h! S: ^$ D
         
- l9 G. \# c1 U- l* M3 Q$ Y        else, L7 W' r! v: ~- g+ z
          { current_steps.x--; x_pos--; }
; W- H8 {3 r1 p; \         / a4 [" k$ a: b( x' }0 U) |$ e
      }1 d8 J- ]+ s3 z" p
    }. @: N' P1 {- r& a
    if (y_can_step)) I  ^- P4 l) y4 B$ l& P% E- H/ \
    {
" T( f" g' I2 f* ~1 C! y      y_counter += delta_steps.y;; m% k" f( ^+ E6 h1 M7 x
7 i! K/ H; @$ M/ t1 R1 E* j" p
      if (y_counter > 0)- c1 f: \5 \: \. t
      {
1 d- x7 e- G, _4 P/ `5 l        do_step(Y_STEP_PIN);* a) {! _3 c4 `! m
        y_counter -= max_delta;
/ t- `4 M$ E" N( Z. K( ]6 u0 x
! Z, ~( m) b3 s! q7 y; E" P: F        if (y_direction)
! @  d* x" r" N- s        { current_steps.y++; y_pos++; }
1 Y% f! ~' N( q7 W, n3 T       . |1 J1 h. _/ |% V
        else( i, X. `9 L4 D& y& P
        { current_steps.y--; y_pos--; }
9 {* ~0 S2 V0 Y% ^: j6 k6 V        7 p: Y( i5 @# [8 O- k; q
      }/ o0 h# K2 u$ g' p
    }5 S8 |; L' V, Q% i) L0 Y
2 g: l" m1 F" T& b/ O
    if (z_can_step). Z( e/ ^1 j! X
    {
, d0 y+ U- y6 }, k: W      z_counter += delta_steps.z;
& r7 ~8 r# f  _, M/ S1 C7 w4 ^0 M( L% M2 {* y$ W
      if (z_counter > 0)
" s" {2 z7 b( ~      {
( J4 [5 r& |; p7 f; b/ ~        do_step(Z_STEP_PIN);
+ l  L4 ?% x* ^1 I3 A" G. |5 S4 R        z_counter -= max_delta;
" P, L+ \0 A3 q
* J: J7 O9 N0 h% k- f        if (z_direction)
# j9 [5 g# r6 d' {5 z3 P, i        { current_steps.z++; z_pos++; }& H2 V3 R, `# Z. N! }: v
        
$ V/ ]) t- x! @( C. q+ v        else& b( v$ b4 M( R' n5 {* K) ]! E
        { current_steps.z--; z_pos--; }
. c' b+ b( Z7 z3 q  r/ A9 b. Z        
2 U7 ?: ]5 t# y' [. F" a      }) O! k  v. G2 [) \4 I- j
    }; T: D; G6 \% x. V( Q- o
    : ~7 n+ s6 c* y( b/ n  y
    if (u_can_step)
/ b7 P" h9 B9 c2 r" E    {3 f* k1 r. A5 J  ?: ^2 k4 V+ Q
      u_counter += delta_steps.u;
6 F& Z5 _9 j8 U$ ^/ |; J% a8 }
% r0 E4 s4 z7 Y, R% A+ `# K$ e      if (u_counter > 0)
3 }) U1 K2 r; U* ?3 ?      {0 O5 `2 {% Q) T3 A
        do_step(U_STEP_PIN);
% x( x! d6 e! M5 y% s        u_counter -= max_delta;
) X) w  E9 t, Y) Z3 c; }; l
( H1 ]* h! t* Q; V" U+ L        if (u_direction)
3 n5 z# m7 n" V" D' _! s1 j        { current_steps.u++; u_pos++; }
- S% d) V2 L1 l         
7 y2 f- y, ?& K" b6 l- t        else  `  J$ h9 `$ l  f5 s4 O$ A6 l
          { current_steps.u--; u_pos--; }1 N  w* H: I5 X& m6 j0 W
         
9 U4 D; W  n7 E% L( [5 E      }
+ ?" ~2 i% m0 g% l' O' b    }
, y7 H' D- D) l8 t/ Q& V2 K1 Q    //wait for next step.# e; u% n1 P; Z8 i# a
    if (milli_delay > 0){
4 m6 J8 P% h5 X  W/ z2 \. V- {( ]! Z      //if (digitalRead(BUTTON_SP_EN)) SPEEN();  I3 z3 ~3 K7 K. K/ _% D
      delay(milli_delay);
2 b% c/ y  N9 z  o# m6 y5 a5 z! u    }                ; N7 P& s# v# _" z& s- j* \
    else{& c' q: S! F6 b' Y
      //if (digitalRead(BUTTON_SP_EN)) SPEEN();" r. k) y7 U" J& }' Z
      if(micro_delay>0)delayMicroseconds(micro_delay);" `0 `- n+ ^5 T
    }# H$ i5 a$ ]- L2 ^; ?; o
    //if(x_can_step%40 || y_can_step%40 || z_can_step%40);& }2 G6 X1 z+ e
  }
. R- l% D+ |: Z4 h( }) `, k& m  while (x_can_step || y_can_step || z_can_step || u_can_step);. o4 B6 s* q, B
: m5 u  `) B3 w* ]& b) t
; u) e/ |8 w" ]
  //set our points to be the same
% O* u$ v  t  ~: X; Q: @, Q  current_units.x = (float) x_pos / X_STEPS_PER_INCH;- w, g3 _- d2 B; E) F
  current_units.y = (float) y_pos / Y_STEPS_PER_INCH;" Z7 z# W% }$ I( D1 i
  current_units.z = (float) z_pos / Z_STEPS_PER_INCH;( T! K6 A$ `* A8 f
  current_units.u = (float) u_pos / U_STEPS_PER_INCH;9 f) d' R( Z3 f# x
  0 d* j/ h1 D* K5 |% o
  set_position(current_units.x, current_units.y, current_units.z, current_units.u );
9 D/ D2 {, B! ]6 V$ _* Z9 \5 J2 q
/ D9 G% S" ^- C' f1 I  long x_pos = 0; // x position in terms of absoloute motor stepps
: o) F9 h! |' K' ]0 |  long y_pos = 0; // y position in terms of absoloute motor stepps& m  w' p+ W  L2 V4 I1 i
  long z_pos = 0; // z position in terms of absoloute motor stepps
0 T! r# a  M7 t" w, I6 b  long u_pos = 0; // u position in terms of absoloute motor stepps
9 W5 }/ a5 U7 \: `7 a  w$ ~9 t  calculate_deltas();
, k4 c# T3 [& T: s* K- x  : a8 Q! x5 Q$ r) a2 f5 W3 N) Z! h
}, ^3 N: n8 A; t+ m( E. @  p% |

7 o7 p* }4 g5 i3 q' zbool can_step(byte min_pin, byte max_pin, long current, long target, byte direction)
$ m4 {8 ]* l. S5 i{
( B/ N  e; D' m& w( a5 B( Q+ ]( T' T  //stop us if we're on target
1 b; @% t- L: e0 e& a: w" j; k' O1 g  if (target == current)1 R0 C5 O; k: s+ e. b
    return false;- p& p! d  z# j3 J1 }: {1 q' j
  //stop us if we're at home and still going : r) Z$ }2 O# @+ z
  else if (read_switch(min_pin) && !direction)
: b+ Y% t1 X1 _' s9 j7 v    return false;, G# Y5 G8 C( S6 Y1 B( E; A5 H, k
  //stop us if we're at max and still going/ `" I2 x# I# ?" ^* g
  else if (read_switch(max_pin) && direction)* P1 L$ q8 h1 I' n
    return false;3 A, F/ A8 N$ U% h! z  e) d

0 i4 d4 [9 L$ Z, X  //default to being able to step$ z9 u, c! d! `' k# i$ Z! I
  return true;
! h' k# O& F' M; \, @" P1 i}; [9 [4 {. D5 p/ |" n) }

& }) e' O" x: n8 X( [& c. Zvoid do_step(byte step_pin)2 t" r! P* y$ n8 W1 H* c9 A% [
{$ _) E1 W9 g) t3 ^5 i5 C1 J4 J" O1 }+ o
  digitalWrite(step_pin, HIGH);4 ?  e4 Z5 M" S5 w  m7 ~& V
  //delayMicroseconds(1);0 }1 X3 Y' A* W6 _9 s/ B2 p
  digitalWrite(step_pin, LOW);
- S7 D9 M  F$ S$ ?$ A}
/ N; \: {! q& }# i& h
' K; P3 T" |$ f/ L$ g! w" nbool read_switch(byte pin)
" K4 D: N# i. F' _2 K3 r{
8 e+ d3 p: f8 w) |( u  //dual read as crude debounce
3 p' }7 N, u' t( v% n: ]  _4 @1 u9 x: m8 T
  if ( SENSORS_INVERTING )
  r& V# h7 _! b; [  u    return !digitalRead(pin) && !digitalRead(pin);/ a. r# v* ~( g) R0 w3 _% i5 W6 }
  else
4 _- e7 o  L) ~" [: w$ k    return digitalRead(pin) && digitalRead(pin);
' K4 m; S+ P$ a; ~3 Y9 |}
3 r9 B  q; M# |' l# v: A  v2 b1 B3 q# u3 g4 `4 {/ B7 M! q
long to_steps(float steps_per_unit, float units)- R- E! u) b. Q: a) m7 K7 E
{
$ Q3 A0 i" a+ L  return steps_per_unit * units;5 W# _2 g6 g) K! c& N7 Y
}& ~2 }+ \8 O" Y5 e6 i2 q# ?) P
/ X. q, J' p: T7 d/ v: ?
void set_target(float x, float y, float z, float u)
9 e9 W: p: D1 C( t' v' O. i{
6 D& B+ ?. G! l; N0 J  target_units.x = x;
! T! K, }2 ~  K& k  target_units.y = y;
' L0 y$ Y! ?. A  target_units.z = z;
2 p3 `6 \  D( T# {) f. s  H& C  target_units.u = u;0 H1 H5 Z" }# j' p# N
  calculate_deltas();3 T) ]- A' _  X7 F# X
}' w  _! {* K/ F; [8 Z

7 o7 w# q* Z: G' Uvoid set_position(float x, float y, float z, float u)
5 [( k4 l$ ^# g6 m, B& s{0 Z2 x5 N& C8 B$ \" M4 q/ U8 C! q
  current_units.x = x;
7 S0 _& _  m5 L; d( P* M- x; [  current_units.y = y;- |5 r; `7 E/ Y: v/ x0 U7 I- ~. ]
  current_units.z = z;
: c( v- Z; F+ V- Y  current_units.u = u;( }9 |' I" Z' _4 F  y) c+ P% m7 \
  calculate_deltas();
8 y- `: q# l; s: G* N}" p# q0 p2 a5 }( n: a& D

5 |3 x9 q. e7 O1 e) Mvoid calculate_deltas()
7 R) B1 ~' F5 v2 G: }- M{
! a5 r" w+ ~. V, o( |  //figure our deltas.! x% E6 ^! ^) a! R+ I/ Q: {
  delta_units.x = (target_units.x >= current_units.x) ? (target_units.x - current_units.x) : (current_units.x - target_units.x);
. _' }+ B8 T! d: ]7 }0 }  delta_units.y = (target_units.y >= current_units.y) ? (target_units.y - current_units.y) : (current_units.y - target_units.y);7 U. H7 e' J4 r7 C  ?; m+ u
  delta_units.z = (target_units.z >= current_units.z) ? (target_units.z - current_units.z) : (current_units.z - target_units.z);8 V3 V5 T0 M6 e+ [9 r, m* v
  delta_units.u = (target_units.u >= current_units.u) ? (target_units.u - current_units.u) : (current_units.u - target_units.u);* v+ y& `& U3 m! m4 R$ Z* b
- x7 S& i- G" S) {
  //set our steps current, target, and delta1 w1 X  \# `% y) y
  current_steps.x = to_steps(x_units, current_units.x);
4 Q# W4 V5 V9 @' W: t( p6 o  current_steps.y = to_steps(y_units, current_units.y);
. I3 E+ H: g3 X; c  current_steps.z = to_steps(z_units, current_units.z);+ c. }! ~+ n1 }. @. b/ m( ~
  current_steps.u = to_steps(u_units, current_units.u);
: T$ `" l  d  x4 c  L* N: s. b! T( G2 J" q( F+ u8 U$ z7 b
  target_steps.x = to_steps(x_units, target_units.x);
7 Q& z# N: W0 A' J; K( \  target_steps.y = to_steps(y_units, target_units.y);3 V4 R) V5 P4 J5 A
  target_steps.z = to_steps(z_units, target_units.z);0 E+ i; ^) J; t& s
  target_steps.u = to_steps(u_units, target_units.u);. ^$ G- `7 v3 l

0 W& O" E% S  k- B3 x3 f7 V  delta_steps.x = (target_steps.x >= current_steps.x) ? (target_steps.x - current_steps.x) : (current_steps.x - target_steps.x);' K* z1 k5 s& n' f( {
  delta_steps.y = (target_steps.y >= current_steps.y) ? (target_steps.y - current_steps.y) : (current_steps.y - target_steps.y);6 o8 I: s5 q0 a' Q- ~# R+ \
  delta_steps.z = (target_steps.z >= current_steps.z) ? (target_steps.z - current_steps.z) : (current_steps.z - target_steps.z);
$ L& i$ W, Z  z  delta_steps.u = (target_steps.u >= current_steps.u) ? (target_steps.u - current_steps.u) : (current_steps.u - target_steps.u);- p: h: B4 H& k2 g' }' p8 h
1 E# l- @6 w' _

+ a; I& [) ~- I9 d$ X% C$ c+ u
; v5 A8 L' ?; g+ b, S6 G5 C  //what is our direction' _% K4 x2 ]& d, ]; |4 i# ?
  x_direction = (target_units.x >= current_units.x);
4 j, R6 K: C# y4 ^1 q  y_direction = (target_units.y >= current_units.y);
$ S2 |  G4 u5 m3 L  z_direction = (target_units.z >= current_units.z);! k2 e+ k. @( n; N9 J5 b- J2 p
  u_direction = (target_units.u >= current_units.u);- J8 o" V# U5 r
+ ~% l3 e* ^! m& o# C* N% P' [( l
  //set our direction pins as well
" b* V; V) B# k! ]$ B  digitalWrite(X_DIR_PIN,x_direction);
( |- Y% T, O/ K0 A6 P( q5 B& v  digitalWrite(Y_DIR_PIN,y_direction);
  A+ a. e1 @) ~) B& v9 l  digitalWrite(Z_DIR_PIN,z_direction);
6 N9 y+ z4 o$ Q4 i  digitalWrite(U_DIR_PIN,u_direction); / n8 `( f; ]- @/ a+ k+ q( b
, z3 C$ Y+ i9 |9 _$ u5 D
  //繪制LCD! c& t5 k6 N2 `7 w8 Z1 ]
  LCD_DRAW();
) a+ D1 L+ [  C  C3 `) @) o4 p+ S6 C) n* u! z# o
}
3 f: r' Z$ n+ x% [3 q) q' _6 P1 g0 J
1 Z& E- W- G, }: e, E& C+ _
long calculate_feedrate_delay(float feedrate)! ^, T6 u1 ^7 e" R+ W  R, O! a, W
{  ^& v, V! a; m
  //how long is our line length?
( t9 s, w" M' u, R# k$ O  float distance = sqrt(delta_units.x*delta_units.x + delta_units.y*delta_units.y + delta_units.z*delta_units.z + delta_units.u*delta_units.u);
9 K5 }& L( ]! c4 K6 W) h  long master_steps = 0;
5 a: s  |: j* F9 J/ N" }* ]2 f4 g
master_steps=(delta_steps.x > delta_steps.y)?delta_steps.x:delta_steps.y;! Q& d5 Y/ O* Y4 j/ O$ ?6 s0 T
master_steps=(delta_steps.z>master_steps)?delta_steps.z:master_steps;
4 g- h6 E* J& o: c& Smaster_steps=(delta_steps.u>master_steps)?delta_steps.u:master_steps;7 E" e+ S& K2 v; N0 ]6 d
0 W$ C9 h0 X$ M# t5 m6 G9 _! \

) G! Y0 q' f  i3 _2 s  ]) r
. t& F* t8 K% }% T4 a) F. \1 q9 |' i : z* I8 M7 Y) Y$ t+ e
  //calculate delay between steps in microseconds.  this is sort of tricky, but not too bad.# ~: U/ j4 y2 w' u- Y$ H
  //the formula has been condensed to save space.  here it is in english:9 |: _" W# L9 T' q. L  H
  // distance / feedrate * 60000000.0 = move duration in microseconds
* z) b3 f) m+ M4 H! U' O' q  // move duration / master_steps = time between steps for master axis.8 e. e% M0 E5 b4 ^) X( n5 u
return ((distance * 6000000.0) / feedrate) / master_steps;: h6 s, j  N. @$ _
3 F- I! I3 X# }) z5 v
}
+ U8 C8 Z9 h* R) L. i0 d+ [# p2 ~9 B! K7 ~
- F0 S9 O  \: x* I5 V" @3 ?1 Llong getMaxSpeed()7 i5 {/ q, E+ p& ?! ?) k
{& [5 `; ^  q7 E9 r( f7 k; _
if (delta_steps.z > 0 || delta_steps.u > 0 ), l, B; M; l; v) n) v2 n8 [
    return calculate_feedrate_delay(FAST_Z_FEEDRATE);" p" O8 d4 f  @# C% a
  else  n! v6 @5 Z- y. R% G
    return calculate_feedrate_delay(FAST_XY_FEEDRATE);0 U# ~! z, l( S
}# v: S$ S0 |8 P6 ?

) G3 B8 E! Q$ S9 ~2 F: Lvoid disable_steppers()
) ~% H3 x# l- Z8 w{
+ m9 A* H7 x$ A. V) \5 d  //enable our steppers
& s0 D+ F# \- i" H  digitalWrite(X_ENABLE_PIN, LOW);
/ Y' A. j6 l7 Q! I: y4 b7 n: ?2 t  m  digitalWrite(Y_ENABLE_PIN, LOW);
$ {& u% B- b& d0 H  digitalWrite(Z_ENABLE_PIN, LOW);
2 F; W* a' ?; ~6 W) x, v, k  digitalWrite(U_ENABLE_PIN, LOW);
- V* K: j$ f! s8 I}
# w0 i1 p7 w; |* U, E
& L/ e/ v" V  a- h+ Q
; Q1 h8 C. r* ?% @( M//繪制LCD
. Z7 |. ~& p- B$ E( X//unsigned int DRAWCount=0;   w: @, D1 T9 r3 @2 n* z* W
void LCD_DRAW()7 A9 K) s6 `- e! m+ J) ^  f
{7 C" n8 f6 M" c! _5 y
    lcd.clear(); * V1 y' w+ n; U- k  ^( \+ T
    lcd.setCursor(0, 0);4 d! K  r' }7 e5 m! P6 e
    lcd.print("    X=");0 v- {# A" v6 n. f
    lcd.print(current_units.x);
1 S3 g; X3 [4 R" O    lcd.setCursor(0, 1);% o% u1 ~. {7 E' [( i
    lcd.print("    Y=");4 S: O( N. D( w
    lcd.print(current_units.y);8 Q: b% l: Y: M/ @
    lcd.setCursor(0, 2);
  x+ n# P- `5 J6 U$ h& Z    lcd.print("Z=");
1 D1 l1 ^, W4 j7 o! V! `    lcd.print(current_units.z);
, p3 o$ S6 q( Z( Y    lcd.setCursor(0, 3);
) }- M4 q: J6 R- ]    lcd.print("U=");
" C) D- n- p, R3 d: q0 F& Z9 f  [    lcd.print(current_units.u);4 Z3 v7 [: b: r: d$ D( _1 G1 \
  }/ W6 w* `$ `2 i( ~! g0 I; \
// else if (DRAWCount>=30)DRAWCount=0;
5 t, c5 J6 Y7 S0 l5 F//  else DRAWCount++;: h, |* q2 L- g+ a( V1 Z
void SPEEN()
" C$ D1 Y  m8 P {" p1 D0 _9 {! K5 C: b5 D
   delayMicroseconds(analogRead(SPEEN_CTL)+1);  k, `; t8 x; ]) q( o* g) S5 V
}
7 {  g6 o4 B, w; M8 t" O
  k* E# B. H5 h% E/ z3 v+ M" R
! i. L( m8 u* u# M7 m  z! ^ //delayMicroseconds(analogRead(SPEEN_CTL)+1);/ G) T% v& f' Z+ w6 _
//if (digitalRead(BUTTON_SP_EN)) SPEEN();4 b) R1 V; V2 [% E, }
- P# @+ a3 W, {2 a" G/ F0 Q! G$ ]
/ f& {! ?6 N* q3 M- g/ i1 x
9#
 樓主| 發表于 2014-5-10 09:58:04 | 只看該作者
基本原代碼的大概是這樣,我是斷斷續續搗鼓的,玩到最后版的,后面我會找出來先測試下過,再貼上:)
10#
發表于 2014-5-10 10:48:17 | 只看該作者
請樓主發一份給我,感謝!178354773@qq.com
您需要登錄后才可以回帖 登錄 | 注冊會員

本版積分規則

Archiver|手機版|小黑屋|機械社區 ( 京ICP備10217105號-1,京ICP證050210號,浙公網安備33038202004372號 )

GMT+8, 2025-7-27 16:12 , Processed in 0.114325 second(s), 15 queries , Gzip On.

Powered by Discuz! X3.5 Licensed

© 2001-2025 Discuz! Team.

快速回復 返回頂部 返回列表