机械社区

 找回密码
 注册会员

QQ登录

只需一步,快速开始

搜索
查看: 23936|回复: 14

运动控制入门篇GCode_Interpreter

[复制链接]
发表于 2014-5-10 09:39:53 | 显示全部楼层 |阅读模式
本帖最后由 xmdesign 于 2014-5-10 09:42 编辑
+ I5 O( R; D9 Y8 Q- K* L2 _! o4 a: ?. {7 e4 r
首先声明: Q& l3 Y  J) _% b
我不是专家,业余捣鼓
" {4 |/ @8 ~  ^7 u* L6 n源代码首先来自网络9 i2 c6 B, s+ l9 d5 o( }: X
开发平台Arduino及Maple
; n0 n) P0 {5 e0 \4 _http://www.arduino.cc/
2 o$ t4 [# u; {http://leaflabs.com/devices/
: u% n" m. e9 Z/ V2 Q! S( r6 S国内活跃社区
. K+ R7 J5 x* o0 Q1 Phttp://www.geek-workshop.com/forum.php
' O1 Y& ], C5 d, ~3 u5 C& j# {3 A) S' |8 m
竟然缺少积累,首先寻找最大的开放式开源平台,这样可以积累全球范围的创客和DIY者的脑力活动经验: x; H0 s$ ]2 l6 w0 ^! p. \
来入门了解熟悉思路架构等) e5 |# i/ t- O* c4 |- H
$ K: `# }, L. x# ]" Z: I
我只捣鼓了下8位机Arduino 移植到32位机Maple 方面一点点事情,
2 D7 U* l3 P; D2 }! N许多功能还木完成,不过作为低档次得应用可能还有可能" O( {. I7 H4 Q- g" I! e

( N. U4 K+ \% H7 I' E$ z6 ~我自己也是个半桶水,再乐意玩的起码自学能力还是要有点吧
- m3 X7 i8 Z; _$ v8 T3 b& Q. m+ I. @7 q" R& ]8 _# r
拒绝 所有的求
# t% l0 ~" R  U. ^" }求人不如求自己 不然木玩
* ]" \' D% K5 S) y% W* y. {1 U! N4 a7 e
高手绕道 谢谢!1 W3 Q' y" D) `
, W" k6 n, `9 P+ v8 V5 B
回复

使用道具 举报

 楼主| 发表于 2014-5-10 09:48:37 | 显示全部楼层
本帖最后由 xmdesign 于 2014-5-10 09:52 编辑 7 y' R, L) j- ~5 h& r9 D; B
; a4 b. @9 _8 N' o" O' k3 R
GCode_Interpreter是比较容易懂的,木那些寄存器等虾米开始不容易懂的东东1 y: M' u9 K8 T" T& d3 A# ?
贴代码先" E( B- w! j# y, q; H
直接Maple的,某宝许多超便宜哈+ _6 d) D! C' k. ~' ^
晕,我怎么上不了RAR?, z6 B7 w- j0 N# w
想玩的留下 e妹吧! o" O. F% L5 ?
第一个妹麻烦传第二个妹哈
) b& p8 x( U& d4 `我平常杂事多( j  k" O! b$ `4 d. ^% x6 e# x2 X; P
谁放网盘上再,麻烦开放下
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:52:53 | 显示全部楼层
// Arduino G-code Interpreter for Rep Rap2 t) U: @, z& T& J! q6 Q
// v1.0 by Mike Ellery - initial software (mellery@gmail.com)
: z" g2 i1 x# r" n4 g; w- V// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)8 c" e+ |  D5 }  J- U- L' M6 K; n
// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com)  W+ A% [$ _" v# ~3 z2 a
// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
- z0 ^7 s8 _/ X9 |3 P7 J
! h* |! a9 {* O& |& `, G& q// Arduino G-code Interpreter for Macro / Micro photography- R" J) Z, v" K  |1 D$ L! E8 `2 u
// v1.4 by Gene Cooper - modified and setup controls for macro / micro photography (gene@fourchambers.org)
- C# W& t4 G: C& k6 r. I3 R//modified by YaoHan China 2010.2.15
) X$ ^$ u* i- s7 Y+ @- m& d" S//modified by JiWei China 2010.2.22 (jwdesign@163.com,QQ:75990175)6 ?& _" J* D% V$ K0 C2 a# M' k
#include <stdlib.h>+ M' x* ~( w  m# \9 q, i, \, ]) q: E
#include <LiquidCrystal.h>- o( |" L6 c2 D& h# e* V. N: K

6 o  P! G" O/ Z3 s* s; ]7 L1 p//启动(高电平有效)/暂停(低电平有效)  默认高电平2 W( _+ Q/ G8 q1 ^+ [
#define BUTTON_CTL 35: D: v7 n7 M3 q  j7 c
1 o' c) z0 f  t$ q  q+ j9 S
//点动模式键(高电平有效)  默认低电平
6 C, w4 p' j/ @/ M+ p! ^8 k2 `#define BUTTON_SS 36
( U9 h% p- t5 @0 [( z3 d! i; _7 f9 _
//点动键
+ d% z$ B* O1 L& h  v#define BUTTON_MAN 15
3 c: x. h7 _% }. H- M
% O: b7 V0 f2 F- G; p9 N//电位器速度控制3 s1 T5 L# |# e- P# Z& q
#define SPEEN_CTL 16; U3 B$ K/ B- A' Y$ k. g" t

1 T0 N* U- M  @* v8 K  Y//手动调速使能+ @7 Z! M# P3 k% `( C& o; {2 Y
#define BUTTON_SP_EN 170 D; h$ \- o( l+ z

1 x* b7 `$ w6 Q. s//LCD 类型 1604或1602; x/ V& L7 i* p) T8 v8 ]
#define LCD_TYPE 1604- L, P2 L8 u' q6 n: S6 j3 `8 X
//LCD 引脚定义
* ]) p6 S$ `: I1 {& t#define LCD_RS 23
/ [7 l: M. j: T/ F- o8 v# ~2 D#define LCD_EN 24/ J3 n5 |1 `  Y) l: q& Q
#define LCD_D4 25
! V6 H- V; u: E$ I' R#define LCD_D5 26
: E2 q3 u5 J! E. L: h9 p#define LCD_D6 27
. o9 ^1 [& G, h#define LCD_D7 288 I. v* d; n+ r
) H4 \$ W; Z2 ^7 w6 H
//命令字符串
9 r3 r# ?9 i7 B4 t#define COMMAND_SIZE 128/ W. q7 k1 ]- j( d8 {
char word_old[COMMAND_SIZE];  {& F7 Y3 d* o- @8 r
byte serial_count=0;
- \. {6 \, i6 N4 q. r4 ?/ L# Rint no_data = 0;
( g/ Q1 i+ G; f+ y3 ]" ^//LCD 引脚链接 配置) P% C. O$ r. b- a* ]4 Q7 O% T' K
LiquidCrystal lcd(LCD_RS,LCD_EN,LCD_D4,LCD_D5,LCD_D6,LCD_D7);- j! e& e4 @5 w* I0 [/ }
//停止控制,高电平有效
2 Z& K7 E/ Y7 J7 M#define BUTTON_STOP 30  h2 C) u) W# @3 p  L  C$ p  _, n7 b
int stop_flag=0;
! H' I0 L# ?+ N' d7 @9 j
0 L+ M# L% e  |6 I//暂停9 |8 l6 y8 s3 t& P
void pause(){0 L& f1 g+ m+ U) _7 M7 ~
  while(!digitalRead(BUTTON_CTL));
3 n: S% q, S1 e) {}
6 x1 {$ C$ Z; h, P+ R1 V4 d
- k- m8 N. a' E0 A- q8 Uvoid stopper(){# u$ t4 |  W, L
  delayMicroseconds(10);
" L* [0 X. v1 n- ?' h. ?2 v; T  stop_flag = digitalRead(BUTTON_STOP);
& D( o# S# o- i) @3 d8 f}
7 ]/ X9 e9 E9 q2 G) C
8 P6 @" y/ y4 j; avoid setup()) ~4 E0 p3 h( q5 z" f3 n
{
3 A1 S. d# @8 [- a5 `  //show start
7 }7 C, M' H" R  SerialUSB.println("start");4 _9 e  [/ w' k+ B! H9 n. A% ]
0 N/ X5 Q3 U! D3 v+ `( b* q! {
  //启动lcd
7 N! S3 b- H5 M5 [    lcd.begin(16,4);3 g# F1 S- H$ [; p. k5 z9 i' _$ ?
    lcd.setCursor(0, 0);
/ z' `$ e  C7 x1 g$ _* |1 k8 f    lcd.print("hello, world!");
) q0 k& H( q" {" J# r
, U% F  N8 l8 X8 z. Q( c& `  //初始化控制引脚及引脚功能
- J) C  t& e( I* z  t  pinMode(BUTTON_CTL,INPUT_PULLUP);
. J0 M: Q4 r0 H7 Q* m  pinMode(BUTTON_SS,INPUT_PULLDOWN);, t* |' z3 k7 S( ~7 K5 j
  pinMode(BUTTON_STOP,INPUT_PULLDOWN);6 F6 k8 l$ y. ]- }2 L
  pinMode(BUTTON_MAN,INPUT_ANALOG);/ Q) S5 N) z7 a  E

  S( Y; q2 \# X6 v% X  //控制引脚的中断
( P2 K( y2 W1 d& m+ c( p0 N  attachInterrupt(BUTTON_CTL,pause,FALLING);//暂停的中断
1 t9 a7 j# ?& d  attachInterrupt(BUTTON_STOP,stopper,CHANGE);. Y8 y* q( k8 Q/ N, Y# Y7 [& c5 @+ w
% q# N3 R) g- y1 U5 X/ w  m2 ?
  //other init
, u- A7 C( G0 P( p  init_process_string();8 w2 w: U. `  A8 [. N# c
  init_steppers();# D" B) E8 H. |1 a7 m1 f; o
  init_camera();5 a( o6 k' F4 T3 v' d! t( W

0 k' P+ [& y, B2 v* R, E}" N: w3 ~7 n2 L9 D# R# X7 W

$ r1 Y( D  X6 E& P; zvoid loop()
' V8 P, `7 a8 O7 S% ]* ~( q. X8 h{! E9 R' D& b5 `7 N& j/ J
  char c;# e& i" R& ~7 I! ]" R5 Q, X

; q8 W8 J% x7 T/ @/ m  //读取输入的字符
8 ^: c) D3 d: x  if ((SerialUSB.available() > 0) && (!stop_flag)), m2 G5 O$ a$ W$ ]9 q/ u7 c/ b2 P
  {
7 |+ u, x: x4 z6 v4 Q$ P- R- p" V    c = SerialUSB.read();
) [! w6 F( f# B) @" r    no_data = 0;
2 g. ?4 u) p2 f3 W3 ]* I# F& C
+ T+ g6 W+ N9 s) w5 d    //换行符代表一个命令的结束
% Y7 O  l0 r- @9 }  S. s    if (c != '\n'); \% K% t' z1 _
    {
. T7 j: Q+ p- W1 z+ {9 ^      word_old[serial_count] = c;
5 u! F6 `; P  B      serial_count++;
* {* G0 C5 \8 J, _) A9 M& a. G# U
! J6 z! t+ b$ B$ x, p% |, F( l    }
; O6 [& u1 N4 t5 |  }
. {9 O8 [4 V1 H' E) s7 }  //标记没有数据输入! A3 m6 R8 ^9 ?0 G7 j
  else5 D0 V) |+ j3 `6 c! i
  {3 W" @. B" k/ @% A
    no_data++;; e' d! t$ u* q7 u0 g
    delayMicroseconds(100);" z5 N/ w/ K# W* n
  }
# a- |% |# l3 L) {8 y% `) s1 b
5 h" _3 [7 Y7 T% k% R  //if theres a pause or we got a real command, do it
1 z& `- G  ?& x( w  if ((serial_count && (c == '\n' || no_data > 100)) && (!stop_flag))
7 b& Z  B& ^7 @  |, t: r  {! u3 @" t' o' @

! G2 w( W/ j) Y+ \; D% D6 s9 V    //处理命令
# x8 M3 p; q9 a+ P9 h! Y1 Y    process_string(word_old, serial_count);; G( G. o( B' A, j2 l" M( E( E
0 ]+ t* n# x, E$ d, c# V4 S
    //清除命令
5 }9 Z, H, G" T+ R8 {    init_process_string();: C' v. Z, ^$ q+ q5 m) F" l
  }9 s* N0 o& a  @- v7 Z6 A
8 S/ N" j& C: q+ W
  //如果没有数据关闭电机
  n/ J- e8 ]) W  if (no_data > 1000). }7 M0 Y/ m. h
    disable_steppers();* j; I, i' z+ I& U8 _

; O4 x" H& \6 d. s' b1 p  Z  //如果ss键按下进入点动模式5 p& m' J- y; d: h1 N3 K
  //if(digitalRead(BUTTON_SS)) ss();
( G" D- B) u0 {  J3 I7 \  K/ r  run_a();7 l- c: H( \& _1 d5 G
}
: A9 p, z& r8 U( d& `2 C( H, H7 D- S5 W( N$ R4 M: J
//点动模式
( }' L" u6 E: wvoid ss(){% \$ R. A; D& @. E2 l( Y

1 |' x' D, V  a- f! g2 S4 I& W, O  delay(1);
* I% F/ _' [3 p6 s, _  if(!digitalRead(BUTTON_SS))return;
$ A' g; B( n$ ^- F) K+ R  if(!digitalRead(BUTTON_SS))return;
+ G. F' H* f3 ^  L) ^5 }  //init_process_string();
) h$ c+ }( y% S! {) o* D9 j  //init_steppers();8 _1 a' }- c% V+ t/ Q, F5 r" G
  //init_camera();5 h2 k& v3 @; K1 f/ A
2 {2 o3 c* Q# T+ {: g; E
// SerialUSB.println("Step By Step Mode");
' E' Z3 M, V. ?8 A//#if (LCD_TYPE == 1604 ), {3 E3 _1 U  ~2 Z! d
  //当LCD为1604时显示要处理的命令* ~. s' [% @+ _+ G6 V8 ?
   lcd.setCursor(0, 0);
: V$ O% ~! R( i8 R8 ~3 l( z6 `6 Z1 @   lcd.print("Step By Step Mode");
# l2 x& X7 c  B  y2 }( D9 `//#endif
* E6 P$ t6 G( N& l! N/ C' g! z  process_string("G1 F5000",8);. o) i! i& e+ F2 x1 d9 y
  process_string("G91",8);
! G; s; W/ H* i; E* Y' N  //init_process_string();' |, o2 Q. Z. J0 @3 H% R

+ i/ ?+ e% J: J3 T# Y) z& p. b  while(digitalRead(BUTTON_SS)){
9 C( K4 x1 |9 t& B    int i=0;* l' n2 K$ W" C0 U* t
    i=analogRead(BUTTON_MAN)>>9;
  F: w! w3 X5 a, `' {    //if (i==0){break;}- K: }! j0 k5 j3 d5 }2 X* m
   //SerialUSB.println(i);) m2 S+ M( J! P: z# Q5 e% A
   //delay(1000);   . g- R: h; a' F3 l+ C9 c
& O2 ?) a7 H* s' e
   if(i==2){
$ {0 V3 S+ s1 b$ b3 q* p8 j7 r& @. b      set_target(1000.0, 0.0, 0.0, 0.0);" C* Z1 S8 r" i2 R" w8 ]4 Q
      dda_move(getMaxSpeed());) v$ R) w$ M6 m' @1 h# I; c
      if(stop_flag) return;
# h2 G. \4 h. M, g      //process_string("X0.01",5);. a7 ^6 Y0 n- _, q* i
      //init_process_string();
  T  s7 R$ l$ i! G  b/ W( o& C    } + p' i  d( H0 K; w) n. P
    else
# ?+ t" U( @/ V/ Q0 t% y0 N  Z    if(i==5){
! M' s. D* n0 y4 x& K3 t      set_target(-1000.0, 0.0, 0.0, 0.0);7 r# x! a+ l" Q0 q" q5 I
      dda_move(getMaxSpeed());, B" y$ k% S+ K: D+ O5 Z
      if(stop_flag) return;
. n5 C% ~( P  _3 j0 g6 y( N6 F( }      //process_string("X-0.01",6);4 P( B' R: q* O' c
      //init_process_string();6 l3 s0 w; v1 s2 o; }
    } 6 p# T, c& O8 n# O9 t
     
& V% v1 N. v  ]: @0 F
9 L* E- A3 c; h+ [: P9 _! K  }  E) \5 C3 }& o
; E: w/ R% n& c' L3 `& ?8 z* O8 t& q
  //init_steppers();
* ~# P9 s. N9 ]( g/ K. A  //init_camera();
7 a: U9 N; s+ j3 H/ N; I3 ~   // SerialUSB.println("Return To Normal Mode");% O3 g. |. _4 n: i: U" a
     process_string("G1",8);/ q- n2 d+ W) j+ X
  // process_string("G91",8);
" d- f, Q: k8 w9 z  t    init_process_string();& w: C, ?* R* D6 ~4 p+ C
//#if (LCD_TYPE == 1604 )9 S1 e, u/ ^, F; o8 u! N! ?
  //当LCD为1604时显示要处理的命令
) o' n  o7 u, I  v  // lcd.setCursor(0, 4);
) G* k( o8 m6 P3 z( I  // lcd.print("Return To Normal Mode");
5 y7 ?& `) ]! K# B6 H2 \//#endif+ k4 g! ?; j8 t  H
}
4 I5 `$ ^# z, j5 k
0 W4 i& M4 m" n. v* Nvoid run_a(). C% N: D( J3 X- c" h1 u6 l
{; W4 \; U: E0 c. s8 U
  //delay(1);
' o% _0 o6 K% n1 l- V& i$ Z  //if(digitalRead(14)== HIGH)return;
6 N2 w% b0 E* _4 j* D$ K  //if(digitalRead(14)== HIGH)return;) m. Q& i9 {- w! g7 q9 @. u
  //process_string("G1 F2000",8);
  L; H. C" V9 Y9 z //process_string("G92",8);
$ K: J6 B. f' @- B% H6 l" G8 O process_string("G90",8);
1 ~: {5 f3 s' U8 o' L/ t+ f2 } process_string("X120 F10",5);# M: _3 e$ d$ `$ Z" h
process_string("G4P2000",8);
" O) S2 G2 D: Z" `2 I' @; P5 V process_string("X10 F10",5);5 g+ w( j/ E" @9 B6 h, @
// process_string("M101",8);
+ W7 G- q0 j' m; ~* Z' I// process_string("X-50",5);
, y' `) x- e6 Q dda_move(getMaxSpeed());
8 [1 e8 n1 O- {) ^7 w& e( Z init_process_string();9 G! v  r+ ~3 |( Z
5 X' D+ T2 E$ ^0 j
}
" E& g* E; F6 d; y1 l& ?# E9 ~
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:53:25 | 显示全部楼层

- o( v2 ], D- M1 d2 f/ [+ j5 I4 ?- E0 `* v
// 定义机器参数7 ]6 ^1 W& W" Y7 r9 t- A) ]
#define X_STEPS_PER_INCH 400
' S- @, i; N# l- ^" I9 _#define X_STEPS_PER_MM   16.0+ t" q1 p& _$ U% Q, _3 \  H
#define X_MOTOR_STEPS    200( h- [. B; y) G8 s% N9 _1 e+ H7 i

% u9 l7 E/ ]7 a8 @5 W$ K#define Y_STEPS_PER_INCH 400.0
# W% I) f; m/ x. i( r#define Y_STEPS_PER_MM   16.0" W# X# C* |4 W) Y2 ]* v
#define Y_MOTOR_STEPS    200: @) z; a4 r) O* g2 e  o
; q" ~( H5 H1 S. {" @
#define Z_STEPS_PER_INCH 400.0; N* X9 A8 ?8 o8 _  j- H* r
#define Z_STEPS_PER_MM   16.01 o5 K) [! A* g$ T: L7 X* H& j) }
#define Z_MOTOR_STEPS    200
% x! N0 T+ Y, m/ f" k" {
$ O. o+ O7 K5 a9 i  [( i8 ?#define U_STEPS_PER_INCH 400.0
2 R/ c+ C5 K6 ]; D- |# z* q  N+ O#define U_STEPS_PER_MM   16.0' T' v6 A( W, N5 S6 p
#define U_MOTOR_STEPS    2004 {& b% }7 R. G
" l3 A" c" T# i
//最大进给率. a, u, m% B- m3 T
#define FAST_XY_FEEDRATE 1500.0
9 t; p' l: V. p+ I: P+ Y#define FAST_Z_FEEDRATE  1500.0* X( E+ E: j7 c
#define FAST_U_FEEDRATE  1500.0
8 J0 l8 z2 _" Y! ?$ v// Units in curve section( G2 q: X# E' I; j( y+ \' V0 \3 @
#define CURVE_SECTION_INCHES 0.019685
, Z  q' i! Q- r* X/ X1 Q; U. V#define CURVE_SECTION_MM 0.5
, E* c: W! J1 v, w% k( W  q8 B
; ~8 k; I" Q7 y3 I+ L8 m: F5 a
! O! U, {2 D) u, s6 U" X+ z! w// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed), ~- w, L+ M! E+ w
// RepRap opto endstops are *not* inverting.& \! D+ d" Y  O) X1 r
#define SENSORS_INVERTING 1
* |( x5 g# X: O( P8 \7 Z( ?: q. F! B+ j- [( G$ ~5 L$ h8 u
/****************************************************************************************
7 A8 C1 u# w' `7 ^$ Z* ~ * digital i/o pin assignment
: _5 n5 R- [2 |) P *' j( `6 l( W- p+ t" }3 K3 m1 `/ S2 J
* this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5
- ]1 X( p1 s5 u& B- l9 L# ?6 i2 v ****************************************************************************************/
1 I  u) q) ]& ?
/ j- Q& f- k, ]- P/ Y//camera shutter and control pins1 x6 A5 ?; S- t7 L0 }9 g0 }; ?
#define CAM_SHUTTER_PIN1 29
- D& P$ d# B! p( X/ y/ b$ v#define CAM_SHUTTER_PIN2 30
/ ]% `, B* @# T% z//#define CAM_AUX_PIN1 31 // analog 0* k- T: Y, t7 B$ D5 |
//#define CAM_AUX_PIN2 32 // analog 1' Q# z9 O- K8 S4 {  A- t
//#define CAM_AUX_PIN3 33 // analog 2; T# v2 m& U* [, a  [1 B0 X
//#define CAM_AUX_PIN4 34 // analog 36 E3 v7 L: u3 N5 p& i
9 H. O4 Q& c" B- K7 i& ]
// stepper driver pins
! r' V+ W! }' w6 O* h  d#define X_STEP_PIN 78 O; D% D2 Z3 T7 V5 |2 t
#define X_DIR_PIN 8- y' Y; c# E3 v2 Q5 U/ @
#define X_ENABLE_PIN 195 H- S$ M: ]% n* D6 q* `

) g9 M. P  t, Y! P- T9 {, p/ {#define Y_STEP_PIN 9
' G6 H& d$ R0 N, o  ~+ w; T9 _#define Y_DIR_PIN 10: G! }7 j% _6 A5 x% @5 s. |
#define Y_ENABLE_PIN 19. b4 W  h9 S, B8 ?! C9 o& U6 |( i

& k( G2 H* [5 e+ X" [: e! i#define Z_STEP_PIN 11. I6 |2 f5 ^0 P! w, s; B
#define Z_DIR_PIN 12
6 Z2 O% t- M5 V* N& m#define Z_ENABLE_PIN 19* w$ d. D; ^' f8 p6 r7 B
- W( p( {  w- @6 B
#define U_STEP_PIN 13) e$ V; o0 G  z8 J8 x
#define U_DIR_PIN 14# ]) t& _3 r: }) c* ^0 T. [
#define U_ENABLE_PIN 198 D( \* s4 X' _: q, ?9 h; e# E

/ |; R* W" S% i  e; e5 Z1 ?) |// limits not used right now
4 O( o3 X. @( p4 j1 ?#define X_MIN_PIN 14
- Z6 e9 c! W2 C. p) `# w#define X_MAX_PIN 14
3 G0 R* K( ?' s. h$ ?#define Y_MIN_PIN 14
' v6 m7 `. o9 F( S#define Y_MAX_PIN 14
+ H% U7 f3 e4 l+ @& B#define Z_MIN_PIN 14
" I3 h5 _  d/ I' X1 w#define Z_MAX_PIN 14! F: v, f' i! {- v3 O: x
#define U_MIN_PIN 14
' D! w7 t, F; z+ a; \3 k#define U_MAX_PIN 143 q' I. a$ X9 U) n6 m- f
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:54:26 | 显示全部楼层
void init_camera()' Y$ H" N$ T; j) D
{
7 W& @  e* m/ f9 B' [  pinMode(CAM_SHUTTER_PIN1, OUTPUT);
+ }5 w6 I9 ]* z8 S  pinMode(CAM_SHUTTER_PIN2, OUTPUT);  `; P" O; ]  ~& m, G/ F3 F
  //pinMode(CAM_AUX_PIN1, OUTPUT); // analog 0
" I5 W* ~* }3 X9 W, _1 c- Q // pinMode(CAM_AUX_PIN2, OUTPUT); // analog 1
5 i; v* P4 F! R: z- J // pinMode(CAM_AUX_PIN3, OUTPUT); // analog 2
% W+ h' g. j, L. `0 I2 N" Q; J" Z // pinMode(CAM_AUX_PIN4, OUTPUT); // analog 3
& U: `8 _6 a; C# |}6 M! e6 z# t2 K  v+ v: W+ y

& X5 S# Y( J4 Q4 R% w$ |" _# cvoid camera_shutter1()
& w: _0 n  ~0 P8 D{2 H* ?, F% E( @7 y5 Q1 E
  // fire the camera shutter via relay...1/4 sec hold time
( W# d6 D6 f% d' X' N4 m9 o  digitalWrite(CAM_SHUTTER_PIN1, HIGH);6 J$ T7 L+ {9 ?( w: p" K, W6 l
  delay(250);
( v& N1 N# _. F. P7 z  digitalWrite(CAM_SHUTTER_PIN1, LOW);  W+ `& \# Z2 J: |

/ [7 }+ V; G  A" {8 u3 y; D+ y5 ^}& H+ M" H" w, w3 Y; s8 b
9 M9 }7 |$ b' Z: s! k8 K
void camera_shutter2()
* P. }/ [. i; f* n{
4 u! a. G& r0 _4 X# c  // fire the camera shutter via relay...1/4 sec hold time
' g5 l, a! M1 p( v# e3 a+ [  digitalWrite(CAM_SHUTTER_PIN2, HIGH);
) _- f1 X9 o  g+ p" l- _3 }! z  delay(250);
! F8 ~2 `- G$ m& i& x  digitalWrite(CAM_SHUTTER_PIN2, LOW);6 ?1 D: w7 b, z6 S4 z5 q5 c

' Z' z1 d- t5 B' B' V5 n$ ~7 t}# P$ {3 H: Q6 G$ z: X
/*
1 B) |8 T  |9 g) Q$ \void camera_aux1_on(); u& l; t2 ]$ }; [% N7 ]
{% |6 I# i" \$ _* V( T
  // turn aux relay 1 on
% F$ }' K7 B: i7 r- R  digitalWrite(CAM_AUX_PIN1, HIGH);+ f0 Z5 M% ]/ }9 Z  o
}
4 v& b& f; j) P  J( J. i9 j* D% v4 K* n" _! s% E! u
void camera_aux1_off()
: B7 X5 R; R2 l7 }! G{
9 H+ M- F' z  a9 c  // turn aux relay 1 off1 E/ t  F" o+ F* j9 e5 k
  digitalWrite(CAM_AUX_PIN1, LOW);/ D, `& ~/ W6 ?% d2 r# p
}3 {! I2 a( [2 [0 u7 X  j4 ^

1 \4 g7 `0 l8 h2 u% y3 T! ?void camera_aux2_on()/ p8 _- C. r/ q, f6 X
{4 S' `# \+ H) ^( X
  // turn aux relay 2 on
, z' T; q* S7 s  digitalWrite(CAM_AUX_PIN2, HIGH);- j- @" e- k! k' n5 I
}
+ A$ G: o& L* ?) `! r) |- B* ~/ i9 M4 T/ ^! \6 Z9 N& F9 f$ ?
void camera_aux2_off()4 H* a6 E3 ]( D" [; @
{
( y  p. c4 r4 u6 i/ w! M: X6 J% D  // turn aux relay 2 off2 k/ h9 {& S! Z% T5 ]$ |
  digitalWrite(CAM_AUX_PIN2, LOW);1 o; a! v7 E2 _7 U2 }  e, t0 }
}* o9 i. D1 |- W2 Y8 b& Z
( \5 X1 L" A  C. E$ Y# J9 l5 u
void camera_aux3_on()) w+ O" n: N3 `
{
' @# x9 b, e& }  D  // turn aux relay 3 on2 l0 t) `  r9 F) n5 L
  digitalWrite(CAM_AUX_PIN3, HIGH);
/ q& C7 O0 t0 I}0 `5 m5 t/ L8 A+ Q0 T2 A8 a4 R% m
9 p* g$ P' F) U( s' r+ b$ ^0 W
void camera_aux3_off()* q2 k8 p7 g0 N1 o: g
{5 x6 W" m& p. |1 r
  // turn aux relay 3 off  A% H$ i% I; B2 u6 [
  digitalWrite(CAM_AUX_PIN3, LOW);* ~4 _; D/ [$ C* T
}3 k$ F7 j" A8 I! W  ?4 }( }3 o- O
% c$ q2 ~' R2 m% O
void camera_aux4_on()* y+ b' H+ x" M/ R7 D+ E
{
& e: b2 E; C( c1 p  // turn aux relay 4 on
# I4 ]. \1 J8 r% {' T  digitalWrite(CAM_AUX_PIN4, HIGH);
% p1 T. J" x9 g}# u, k  W' ^; [
) J2 q: @6 _- H* f( e: {2 H; z
void camera_aux4_off()
. @3 k8 Y- c) O" L, D4 l$ z{
- E9 {; u6 j) X' E  // turn aux relay 4 off
4 \8 H# G1 H% p- L5 ]7 g  digitalWrite(CAM_AUX_PIN4, LOW);! j+ o1 f2 z$ j7 f. R4 |
}
回复 支持 反对

使用道具 举报

发表于 2014-5-10 09:54:28 | 显示全部楼层
楼主推荐的网址不错3 ^9 q! ]- `/ G9 ^
请问楼主是玩什么的,用乐高玩具吗?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:55:02 | 显示全部楼层
// our point structure to make things nice.
) D" a4 S% C3 p! ]struct LongPoint {7 K. W! S0 h4 D2 q
  long x;
8 H0 ]/ G% z; C# j  long y;, j0 X# b6 u% Y3 x& [
  long z;
2 t3 v1 u( p0 o# Y' j  long u;
1 J& |" K8 O, c4 m. Y3 m& a& w};
% O! r6 R. l6 i0 c, |0 h9 u! j3 ?' v5 V& r9 Z3 S) ^$ p5 f' B
struct FloatPoint {- V3 ]  q# K) B! R
  float x;
: [0 j  `2 ?, c) P/ j* \  float y;
4 c8 p8 q3 m& e$ O  Z/ V  float z;
% D  e" `: [1 p9 a3 i) a  float u;/ l5 |$ J  L$ h
};
/ A; q( W2 L+ J2 [/ y+ N! S  `( k/ J( w! A
FloatPoint current_units;9 Q8 s( j' K; v8 i, c* z6 g
FloatPoint target_units;+ d! L8 R/ }  ?  }: i) W$ M
FloatPoint delta_units;: t( n; h* i9 K$ s: @1 w; x: f
) O1 X; f2 o$ C) t( ~
FloatPoint current_steps;4 B! {( t) I3 K+ M# A2 l
FloatPoint target_steps;
+ Z  c& U) L+ `3 ^! l) iFloatPoint delta_steps;
0 A! N6 d1 W; Q% |( N  ~
1 U2 c0 w( Y, e6 R; }/ hboolean abs_mode = false;   //0 = 增量位置模式; 1 = 绝对位置模式
3 x# h# o- L- [1 Z
1 q2 |; y: e7 Q2 e) Q' i+ \//default to inches for units' l7 k" w2 s% x0 }
float x_units = X_STEPS_PER_INCH;
9 E) G* ~0 A; Kfloat y_units = Y_STEPS_PER_INCH;
3 c( S1 H- J: z/ t: P1 w0 efloat z_units = Z_STEPS_PER_INCH;, i4 g2 V5 }) H& y
float u_units = U_STEPS_PER_INCH;
( l: [8 Y7 t3 D/ O) ~/ U% Zfloat curve_section = CURVE_SECTION_INCHES;1 g6 e2 w* u4 m: v! q: V4 Q
8 r% ~6 w; C! t& a
//our direction vars8 c- ?$ h; Y) r3 Q4 h& U
byte x_direction = 1;. u( {% y3 p! v! G2 q. f
byte y_direction = 1;6 A+ O$ p3 `5 y9 u3 O7 }' D" y9 A
byte z_direction = 1;0 o% I. [3 s% H$ t
byte u_direction = 1;' ]; m0 ^3 w1 A" E8 z# r
5 v9 Y6 |/ _4 M
//初始化字符串处理- E) a# t  p- Z. C! X
void init_process_string()
4 m$ R0 F. Z1 d! N{! @9 g5 C) v; G) q1 ]  `
  //init our command
6 @1 l/ K# S1 U- I+ H& r  for (byte i=0; i<COMMAND_SIZE; i++)
# X5 j1 M4 W" _& O; O, ^    word_old[i] = 0;  F1 x/ h& N" `* P4 v
  serial_count = 0;4 Y; \# I, p2 s5 {* O8 ^% |( F, N
}
) Y1 [% `0 X: s/ U( D6 D/ Y% s) ?( Q* t! I  U. }
//our feedrate variables.( U3 M( ^% R* _6 _2 F1 e
float feedrate = 0.0;
8 ~) N0 V' Z' |" q& y+ v6 Mlong feedrate_micros = 0;
8 l2 M! h7 N# p' y2 D* N* _
9 q% m: Q) P1 \2 Y//读取并执行命令
- L7 E6 h4 I# ^( h3 y. ]" @void process_string(char instruction[], int size). T- }; h$ O$ d& f3 q9 y
{4 c' A  ^0 `% ?
  //the character / means delete block... used for comments and stuff.) i/ E8 V/ h& X. Y
  if (instruction[0] == '/')
! X. l( F- T# Q1 j  o* {" g  {
; F% [3 Q0 Q2 P. d3 n% O7 d; H    // SerialUSB.print("ok");
" N4 B8 i7 x9 G4 E! L# J    // SerialUSB.print(byte(78));
4 e. e/ g, _) K) f) ~9 a    return;
7 u: S: p1 I8 T# {) u' k# U/ S5 d  }
' @' N1 X' h% t9 Q, p  //init baby!( y# y* s& w& Q9 \9 A: O
  FloatPoint fp;, Y0 C) w: U/ L) l, Q  p! D# a
  fp.x = 0.0;" f& |  [; d( l  \) `
  fp.y = 0.0;
* i( o: K' u% E8 E* H  fp.z = 0.0;% z4 y% |9 {# s
  fp.u = 0.0;
  {' m; t& U2 t8 `0 v( [- `, h0 |6 m& _9 p* p
  byte code = 0;
! S- S& j; R1 ^+ M4 B3 y; R- A/ z8 _4 G2 l
  //显示在处理的命令
7 n" j( e6 y1 @# g0 i6 K#if (LCD_TYPE == 1604 )
, y* U# j: `) ?: v' b  L  // lcd.setCursor(0, 4);9 a" O/ |8 n. ^2 Z. P
  // lcd.print(word_old);  k' t8 \7 L1 z' x
#endif
# H5 T, V" O) e2 j' {0 o) U$ U+ ?  SerialUSB.println();
6 {  x' |1 F6 {  SerialUSB.print(instruction);
! ]7 T. r* G9 B( W  SerialUSB.print("\t");! T: `# w; f; B& c9 s

5 V" c, O$ I7 S; [/ e  //what line are we at?0 B8 z" H5 \! w0 p- ~
  //        long line = -1;
5 h5 ^% K' G% s! a' L  //        if (has_command('N', instruction, size))
0 I2 N& m: ^& _  //                line = (long)search_string('N', instruction, size);4 x- D: M; m' q* o
" l6 k( H5 X+ u1 B7 O  ^" S
  /*
* f& ~! f5 k, ~+ E  U1 \' a        Serial.print("line: ");
) O- U$ J6 t" S! h           Serial.println(line);- X/ M" ^+ t; O( \$ G
           Serial.println(instruction);
- E8 [- K( N2 `   */
# y8 F3 e3 w' A- E  R' d9 i3 r  //判断是否读取了个 G代码?# H6 W- o* a+ B$ i1 c" r
  if ($ ]* T, m& C* p8 ^/ A7 O" Q
    has_command('G', instruction, size) ||
0 Q: F8 c7 e) D$ y0 P$ L8 T* u3 W    has_command('X', instruction, size) ||
8 q0 p% Z# x' t6 v5 b+ S0 V    has_command('Y', instruction, size) ||
: U0 I! y" G% Y( ]8 ^5 n. V" }    has_command('Z', instruction, size) ||( G  h) l8 ~7 e8 |% G- I4 o3 v
    has_command('U', instruction, size); b& O, q  H" t4 o. r4 F% `, j
    )) u8 m2 K# i  Z7 D0 M2 n, _
  {0 O6 ]- u! {. W7 W3 G# @) M! v
    //which one?
' Y6 Z" p( M, h7 d$ W4 c    code = (int)search_string('G', instruction, size);3 i  r  E8 ^" d5 h) o
    // Get co-ordinates if required by the code type given
, j6 F  K3 s% T0 U0 j3 ?    switch (code)+ ?" x' O( O3 F9 d
    {7 Q+ y! o" S  |) P" j
    case 0:
2 X; u* s3 x7 |( E# v  F" K    case 1:' x& ~5 a0 ?# E
    case 2:
4 F* V. s; g8 c  N8 L) V$ X  Q    case 3:) ^# v  J0 i: t; p' x- X  i$ N
      if(abs_mode)  l' b" u5 [' L: ~
      {
. G' r- ]' R5 q3 _        //we do it like this to save time. makes curves better.
/ P. z0 n3 q# h2 W4 ]# M7 J9 K        //eg. if only x and y are specified, we dont have to waste time looking up z.
! Q# B, T! }& N1 g7 Q' Y6 J& `        if (has_command('X', instruction, size))
  {4 E, W2 ^3 ~6 M  m0 S+ k          fp.x = search_string('X', instruction, size);
7 Z- n, l5 m  X, I( Y4 W0 O1 n% t        else
- ~& t: w' E+ g0 U0 g6 V* c          fp.x = current_units.x;
% w$ g# R) c+ R9 D' q9 F4 U$ Y9 H' r# _1 y; @  _# s9 f
        if (has_command('Y', instruction, size))0 W# h3 R; i0 g/ d$ `. w' N) ~
          fp.y = search_string('Y', instruction, size);% a, a7 N$ |; M/ L7 Q/ G/ h
        else
6 c+ z0 ]2 d" J. x  l+ y$ K/ U$ a          fp.y = current_units.y;2 u* X2 x" i2 K" E8 @7 d9 b* }% }, [

% D  z& q( T& d" p4 X# x) X: r        if (has_command('Z', instruction, size))* ]( G8 @* i- d9 a% }, |* @) _
          fp.z = search_string('Z', instruction, size);2 C5 ^$ @! V- @' B+ z* @4 c
        else
# b7 }8 [+ T$ O5 L4 e0 Y          fp.z = current_units.z;4 l$ p6 V0 s& m, r! w& |
         
. [6 e% O7 d. S1 x6 w9 ?        if (has_command('U', instruction, size))
. v/ k, k  |+ e) Z' m, U( r) k          fp.u = search_string('U', instruction, size);
) [. h$ r. x/ d" g, r7 Q" i        else
; H* M* X+ U8 J; x6 X5 k          fp.u = current_units.u;/ f: C1 E$ A1 x, A' x2 s
      }
5 k5 S9 {3 c# W1 B      else
+ p. M5 \8 f7 |% Z      {$ o7 K0 v7 ~0 d6 a7 e& H( N$ B3 i
        fp.x = search_string('X', instruction, size) + current_units.x;
5 |. z: [! Y& ?        fp.y = search_string('Y', instruction, size) + current_units.y;
  u9 }) p5 Q# Q4 p6 d: |2 c        fp.z = search_string('Z', instruction, size) + current_units.z;( U+ C+ {! O& l; n7 |  }8 l+ H( J
        fp.u = search_string('U', instruction, size) + current_units.u;
. @0 @( r: s3 q* v      }) x5 h' ]3 |. u! v/ f( Z0 Q
      break;/ |& q0 l# u0 a5 W  t
    }
5 ?0 [" O( g4 h2 t    //do something!
; o7 y$ I( z8 G: u# h- `    switch (code)# q, X3 x$ @% T/ ?8 P& f. V
    {
. \( f) Z. r0 m2 X" Z/ `- V      //Rapid Positioning
' V3 K& l( @% z; I4 y- N      //Linear Interpolation
& C9 G4 \& \6 `5 @) e/ V. D9 Q      //these are basically the same thing.9 y# r) {  [3 }6 ^+ O1 \
    case 0:  r+ ]! h. E! n; A3 K! r) G+ v2 ^
    case 1:
& L  A8 J  y8 U5 v) L      //set our target.
3 x4 o- U* f+ j      set_target(fp.x, fp.y, fp.z, fp.u);( o% w. Z0 Y: s* Z3 }: Y( \+ d
      //set_targeta( fp.a);$ P: I1 b+ V2 p+ Z% J7 k
      //do we have a set speed?+ [9 p  q8 c& f2 P% C! G) C: H! t
      if (has_command('G', instruction, size)). U4 Z  ~/ y! E& p' Y  j& R0 d
      {
0 d- C! M" W9 @        //adjust if we have a specific feedrate.
- S( y, z4 h9 e9 q! m: Y        if (code == 1)# C) J7 Q8 q2 D8 p
        {
+ c1 ~) V- [# z) V4 E" I$ ?          //how fast do we move?! |1 k* x& m6 u) s2 D: u* v, {
          feedrate = search_string('F', instruction, size);9 q9 L2 s. x3 E* @  _$ l
          if (feedrate > 0)
# ^$ |3 X6 h5 w7 \! T" K) j            feedrate_micros = calculate_feedrate_delay(feedrate);
6 b: N7 [# [* P& w  Q! y( a          //nope, no feedrate
5 U' v4 j& T: \, H8 D! h4 G          else
7 t/ ^7 ]5 _' _4 Y, _* g/ T- X            feedrate_micros = getMaxSpeed();/ w/ T& ~& w! q1 i! w% F
        }
+ s$ N1 g$ C5 P' W) i7 ]- q" o& d        //use our max for normal moves.
3 D0 R" o6 L/ m8 _        else
0 a# U2 z' U, b+ g" H          feedrate_micros = getMaxSpeed();7 U$ D; |6 o( H& b
      }5 S8 y& z3 U) c  l% O+ z1 u
      //nope, just coordinates!1 X3 |, U" h8 ^. z. M9 ^1 [' G
      else
, J4 ^6 D5 `( P. j1 c# B9 d      {; A2 K* \3 {" F2 R# L' M# i# ?
        //do we have a feedrate yet?& J. s+ O  U1 x' {8 U2 t7 z
        if (feedrate > 0)
0 N$ S9 u7 h+ W* e3 \, `          feedrate_micros = calculate_feedrate_delay(feedrate);
' V( w$ P* Z- O" q# a: M$ t- h  H        //nope, no feedrate
# g" V8 Q$ z" d% k& a+ A        else; K  N5 x' c  |3 }  F% V
          feedrate_micros = getMaxSpeed();
0 S; D! Q& m% I, l7 H- q' L( x      }
$ y" t% @$ \% T9 w# H
) a0 @: v  V7 ^+ [, J      //finally move./ ?; L+ I+ H5 E% c
      dda_move(feedrate_micros);3 q% e* O' B% @
      if(stop_flag) return;. ~+ M/ E9 W# m
      break;
1 y  m9 w" p7 r* p% X7 E
* j% I8 ~: p) G% X* W* W& Y, p      //Clockwise arc
0 a; l+ W! T* {# D    case 2:, p9 }& \* b( i7 Y6 d3 ]
      //Counterclockwise arc7 x* C6 W  `- Q$ t9 Y3 r
    case 3:
6 S: D$ o% N+ l/ r/ [6 }      FloatPoint cent;
7 U/ E( J+ P% |0 p      // Centre coordinates are always relative
# w1 r4 k( {- b/ k: G& _      cent.x = search_string('I', instruction, size) + current_units.x;8 }( p; p+ d) U9 z5 Z3 l, a
      cent.y = search_string('J', instruction, size) + current_units.y;3 g7 H  y' f7 c& ^% L9 s
      float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
3 l5 h  W7 u/ o; }1 {6 n' w4 V- w* j/ A  @
      aX = (current_units.x - cent.x);$ u+ U7 j( N1 k+ {
      aY = (current_units.y - cent.y);( W5 x1 H( t' Q! D: t
      bX = (fp.x - cent.x);  g+ B1 |5 \; ]; w* p4 B' p
      bY = (fp.y - cent.y);. e$ K0 z6 i# e

8 Y; t7 c5 K8 K      if (code == 2) { // Clockwise
$ c0 n/ m; k7 K1 N- [        angleA = atan2(bY, bX);4 c; ^  F$ a! C& ?+ H2 V' R
        angleB = atan2(aY, aX);
" f1 G9 o8 [5 C+ t) Q      } 1 Q! v" V% y' q, z0 o/ t  H/ p
      else { // Counterclockwise- Z% \' |& E. G
        angleA = atan2(aY, aX);2 I6 {0 T2 M. Z; Y7 [. k
        angleB = atan2(bY, bX);$ F( G/ B, f( {! E; d; |) J* R$ r
      }0 o8 X7 X8 w# b; R7 y: n
      // Make sure angleB is always greater than angleA* l" y! s& t4 v+ O6 o3 K; r
      // and if not add 2PI so that it is (this also takes
% V7 j$ t3 C" W8 i$ b9 i. v      // care of the special case of angleA == angleB," ^6 j  m+ L, \9 H  B
      // ie we want a complete circle): Z7 v! [: T' i1 r' K
      if (angleB <= angleA) angleB += 2 * M_PI;+ `% a& u- a- n1 P8 z
      angle = angleB - angleA;3 P# q6 F; _9 H0 w( p

& Z. I- _* I+ g      radius = sqrt(aX * aX + aY * aY);. U/ e+ x) M  d) a- ]4 k" L0 A4 v, D
      length = radius * angle;
! T3 d$ ~0 v2 M& l      int steps, s, step;/ F1 n& U5 M2 ]1 i$ ~
      steps = (int) ceil(length / curve_section);
2 M( q  M' ~3 P( r: r4 l
% N5 w9 Z. [) C; ^      FloatPoint newPoint;0 u, m; U, p' S4 v8 `4 F
      for (s = 1; s <= steps; s++) {- n% b, Y$ a' G. F
        step = (code == 3) ? s : steps - s; // Work backwards for CW
2 {$ ]! a3 z" F        newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
$ a! ^1 Z7 c! R) q0 i        newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
* ?( J/ P5 N# p  k        set_target(newPoint.x, newPoint.y, fp.z, fp.u);. M, b2 Q' N/ k. H1 @: H! c+ i
, O. W3 f$ c  C% e( }& I3 U, l
        // Need to calculate rate for each section of curve/ i5 D( P$ ~" p; Q9 T2 B. A
        if (feedrate > 0), x% F! T4 _4 [% {
          feedrate_micros = calculate_feedrate_delay(feedrate);# ^5 w8 M4 |# i2 k- H
        else
5 V3 d7 t& a5 Z          feedrate_micros = getMaxSpeed();/ C3 o$ {+ D0 T  C/ h* B

/ @) _! s5 I1 Y        // Make step
0 B2 W2 ]/ E$ K& v* g        dda_move(feedrate_micros);
& S: r! d" g4 R' P. O5 ~# L        if(stop_flag) return;7 Q; i9 V$ g: D" `
      }
" S9 z7 Y2 s0 B) e3 K" y* e
  |% q: J" a* B& z6 ^7 a  k      break;% @8 M) l% K- x/ o0 h, l

/ D- J( y! [" C% ]& D, m      //Dwell
, A9 I$ e6 ?1 K5 z/ m$ O8 h    case 4:
- B- g, z4 s3 Y  j9 @      delay((int)search_string('P', instruction, size));
2 V; j' a- g2 g; t2 U      break;  |, [/ {! s- ^
! D1 Y" a4 ]' m- f
      //Inches for Units7 w! T: x/ n1 `8 L  W2 B: s9 @9 `
    case 20:
+ R" N$ C" |1 O) c# d; M      x_units = X_STEPS_PER_INCH;9 r) f4 U- B, p" f+ s
      y_units = Y_STEPS_PER_INCH;+ C7 e! J, {. M& i+ P9 `: k( A
      z_units = Z_STEPS_PER_INCH;" B/ a" E$ V* J3 t( s" g
      u_units = U_STEPS_PER_INCH;) K5 x- `. A& r8 N4 {
      curve_section = CURVE_SECTION_INCHES;
! h# x' g6 H( k" l% T      calculate_deltas();% f# C) @5 y1 q2 L/ S% B
      break;! _% j; \$ r# @" e

9 _5 `8 p4 }, {0 t6 e      //mm for Units
; J$ x0 Z; ?4 R0 f* l/ a    case 21:# X  {0 w7 ^) j- [; \+ E
      x_units = X_STEPS_PER_MM;
- o$ c# `! j0 u' b' X      y_units = Y_STEPS_PER_MM;
$ I& Q) [5 i" y$ G5 ?  T      z_units = Z_STEPS_PER_MM;
+ {- Q8 u) ]/ Z' G( V% H2 y3 {      u_units = U_STEPS_PER_MM;
4 _  n  m4 {4 L      curve_section = CURVE_SECTION_MM;
/ [* J( @7 @. X& ^. s  D( C1 \      calculate_deltas();
2 d% x/ h+ k7 b" f! K' g. B      break;
& @( L5 t( O" H* Y) B2 a  O! C
8 Z1 o+ Q" D# F7 C* V0 Z: R9 r      //go home.
2 p* \& I" z" V0 K    case 28:4 [7 z: e- N% R, d; Q# a
      set_target(0.0, 0.0, 0.0, 0.0);) e2 `7 C$ H; m$ A% J/ }4 f
      dda_move(getMaxSpeed());
2 ^8 k8 c% I2 a: m' Y      if(stop_flag) return;/ ^  ^+ |8 U  }
      break;/ u2 f$ K8 O; _

' B2 n6 r7 p- f4 Y0 s- y7 `$ f      //go home via an intermediate point.
" A0 h: ?& a. Y: @    case 30:
" ~3 z9 B! `% O      fp.x = search_string('X', instruction, size);/ P) j  D' E. S6 W
      fp.y = search_string('Y', instruction, size);
# v6 U& T2 H! X      fp.z = search_string('Z', instruction, size);
, `: C( A' D9 O4 g. o      fp.u = search_string('U', instruction, size);5 [! E3 h* _1 r* |; g9 ^* S* L& o
      //set our target.( }7 z2 Y  C6 w+ c( M: r: p
      if(abs_mode)4 j. I; A$ {# u1 l  p% N
      {
) ?; P" Q# k  V        if (!has_command('X', instruction, size))
$ w$ h+ K' r# {8 v9 o$ `- a+ L          fp.x = current_units.x;
  W' N4 ~/ t0 G        if (!has_command('Y', instruction, size))* p7 ]' H7 E3 n+ F! h
          fp.y = current_units.y;
8 _/ [4 k' h, K7 P$ }1 ]        if (!has_command('Z', instruction, size))4 }  t4 K1 e: @0 j0 m4 v1 l( @
          fp.z = current_units.z;
/ r# K3 c# A. i" Y$ [0 F! A        if (!has_command('U', instruction, size)). M9 y9 h$ S. s: R- |* F/ E
          fp.u = current_units.u;) S+ l8 y) {6 R; l2 D
        set_target(fp.x, fp.y, fp.z, fp.u);6 e  Z# J3 v  F' ^, ?/ ?- b
        
% V8 |) c) k2 P. Z" s) Y      }
; L( m3 H8 {1 f5 U% H' O  O      else
: C; R8 A' R& h) `* o/ N        set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );9 ~0 q, `. M5 \7 D
      
: [& e- m4 ~3 B6 f6 Y3 L& Y  {      //go there.
+ a6 B. u) ~2 v( m0 K      dda_move(getMaxSpeed());( V& s/ V2 x# X- K# g3 X
      if(stop_flag) return;
) `- l. z8 H$ G3 C; l2 N
8 t% _3 D$ c2 {" u& L" l      //go home.
3 t) M3 h$ h9 P5 N- [: O      set_target(0.0, 0.0, 0.0, 0.0 );
- a, _& W9 q1 w     
1 d' r0 ~7 _9 d$ e      dda_move(getMaxSpeed());
, K# N3 F8 H1 y6 S; ]      if(stop_flag) return;5 k* m" Z0 D- ^
      break;9 z9 f5 z  @6 R# J# j& x8 S

3 X) ]+ K# m+ [. s      //Absolute Positioning
7 E( \4 }' p: j, B    case 90:
2 a) k6 Z5 b4 X; d( F# n) M      abs_mode = true;
; @! J+ h  k4 E9 P4 N( r0 S2 J  r      break;
: Y' L9 q3 q( o6 @# _& E9 U1 }& L7 i/ G- k+ h
      //Incremental Positioning
( i9 Y; R+ p8 Q! K    case 91:
) x/ ~, O7 Q& [      abs_mode = false;7 q0 }8 p/ s2 h8 _
      break;. ^( Z. i0 t4 l
2 q. E" ?/ P! {* a9 E; c6 I  \
      //Set as home
# X3 e, Q  Y$ M9 l" Y* T4 m1 H    case 92:* j* o4 A7 A1 a2 Y% @
     
' }# p! F) L. C# |1 T0 g0 r) Y5 x      set_position(0.0, 0.0, 0.0, 0.0 );
+ c+ b- F* ^) U) q& e) _      
0 {  P0 ]2 u0 l7 Z! u) {      break;
+ z% t' T: {0 K: ~1 ^
, S2 V/ ?3 c, R' I5 {      /*! K( ~  G% _- E5 ~+ J
                        //Inverse Time Feed Mode
  p: H" P1 ?  `                               case 93:, L- g" Z# p, ^4 y# n. p
      
- {0 t, r! X. Q7 f$ h% C                               break;  //TODO: add this
  g, h; Y6 B: a( N  _; I+ [( I      
4 m' Q4 \: @& @: x                               //Feed per Minute Mode6 Q. Q' I# q( z3 h; H
                               case 94:3 `) T5 m/ q2 V5 t
         W2 a4 |) U/ o( |3 T
                               break;  //TODO: add this
/ t) t; E4 W% V3 f0 P' V0 I       */- A8 }# H1 o( [2 ^8 Y  c" k
' e0 ?) s! q+ v% L( G5 ?1 @
    default:
3 v3 [' @# o% L1 R9 K      SerialUSB.print("huh? G");. x2 n9 u7 x* [: E9 [9 m7 T
      SerialUSB.println(code,DEC);) z2 M% j) N1 _
    }
0 S+ T2 K. W3 {. K. D4 R  }
/ {3 V8 M' N+ h7 N9 l/ ?
! m2 h- }) m  A; b( \9 }  //find us an m code.
. Y; ]! H- ]" p5 K8 b5 D  if (has_command('M', instruction, size))
5 K3 z2 q' F# i  {- m$ `4 h# _4 ^, G0 \- o
    code = search_string('M', instruction, size);
8 d7 n5 {( }( }/ g    switch (code)
5 U* d/ o% r% E: L) \1 k    {
! w, D( q) f* A. n6 t! L      //TODO: this is a bug because search_string returns 0.  gotta fix that.2 {. Z/ O1 \+ o6 z
    case 0:  {! P1 T- s5 o# v% y3 }1 {
      true;
5 x, Z- ^- O% F1 M5 a2 ^      break;
- A+ N6 K: c* ~; \% M6 f! m3 i8 m. h1 y# S: f6 Z! ?8 t& @
    case 100:, H6 o, r* r9 K7 x
      break;
  h7 `4 C$ n! `% Y; \) _( U5 p6 p1 s3 U# ]+ R/ D0 N! Z
      // fire camera relay
1 [* |8 @6 t! F! W6 Y    case 101:
: Q# J; w; ]( Y" _4 O" n2 @      camera_shutter1();4 p; a+ W+ ~& c" u  h8 t
      break;! u& E9 M. W( R
( _2 t1 W& P2 z. }8 ?1 v
      // fire camera relay2
* T! f/ Z. F' N    case 102:( r# h5 Q" m2 _: M# @
      camera_shutter2();
" A1 G: ~9 _- ^      break;
4 |5 t( k* n8 p; j1 F8 \; i6 M$ U/*
% X: r+ _/ S, Z3 [      // turn aux 1 relay on. G, {. ]) T( _% g
    case 103:
" z% ]2 F) `7 ^; q7 D1 t# ]+ W      camera_aux1_on();6 ^7 {1 u8 N: y
      break;# q: l' o% U( Y' \3 f) o% n
% L; ~1 C/ g7 _, B7 H  ^% v( S
      // turn aux 1 relay off
! j6 J6 M" K. @' s. E- `3 `    case 104:
3 F% o" {( ?1 R( D& s2 d% X      camera_aux1_off();
: C  w. |5 ?) X      break;
5 R1 K# W8 R9 d5 L9 n
8 t& p9 R* {& r3 j% @; a      // turn aux 2 relay on
' b) k# P  R5 F! ~4 n    case 105:- C6 J' ]$ H; y8 G# s! D  j
      camera_aux2_on();
; K- G5 w1 r* }& a      break;& y1 K* K$ y& R' N2 C1 M

: V" H: `+ O. B- I1 G6 s      // turn aux 2 relay off
) D) A  y8 B# t9 Y" ^    case 106:4 F7 Q$ G5 w* |) s
      camera_aux2_off();6 K% z& r- z' v
      break;1 l4 P- ]" @; ?# k/ j
, {: F5 w: Z$ s/ L8 L
      // turn aux 3 relay on/ A. N# N% ]3 ?( h* ?( e5 \) H8 d
    case 107:
  Q: f8 r, {: F* A$ o% G9 u5 o      camera_aux3_on();$ w* w  Y% `0 e2 t
      break;. v8 n5 L. X+ _2 q: H8 v9 K- y

, ~2 P: u. u+ c# G* Z      // turn aux 3 relay off2 H' _8 P; A2 K! ]. ]2 K
    case 108:! g! |% X% ?3 o) F& P+ A
      camera_aux3_off();& |: c/ e. {1 `' u3 Y4 Y
      break;& v6 \  i  a1 K3 g) T

6 p* R/ W8 e1 T; R      // turn aux 4 relay on0 S5 T- p" `' D0 d2 C" K
    case 109:
7 w/ O4 X% W) L( X      camera_aux4_on();
$ E2 c8 U# F* u9 n- n& [      break;
' d. p. z; q9 e2 g" f# q( Q9 p$ p+ a
      // turn aux 4 relay off. U# H5 k/ U* A
    case 110:  J% s. v. G$ J. K6 f
      camera_aux4_off();' r: E1 f" E2 f/ }3 X4 x2 q. p
      break;+ ]9 V" ]4 F1 r, D
*/
/ _) P$ P% l! B" n% F    default:
; H# T" ]/ D/ b
  H9 h% J  [( {: j& ]      SerialUSB.print("Huh? M");
. {2 j; o; k( P      SerialUSB.println(code);
( E# O2 U$ u5 l' c    }
5 ]3 P# s0 z1 t9 [% y, I4 r9 |  }
+ Y& J) V; z/ D' g+ B/ S" Z) {( p: D& M: Q
  //tell our host we're done.
; Y- a5 K7 G% ]4 P  SerialUSB.print(byte(78));1 W0 c7 ^3 Y" b6 N0 u
! G7 P7 F) b, U/ ]/ k
}
. v4 K$ q8 Y3 Q' B9 Y6 F  p, c* k% i7 x& z" m& y
//look for the number that appears after the char key and return it) y3 T' _, R: b( S
double search_string(char key, char instruction[], int string_size)0 ~4 M5 d4 B. b3 R! o
{* D: F5 @+ v3 o
  char temp[10] = "         ";
* f. L$ K* n) \8 @) S  for (byte i=0; i<string_size; i++)" {4 B7 t# n; r( t
  {0 O% Z7 z; Z- ]2 ~
    if (instruction[i] == key)3 S& P: f4 H) e4 ~$ ^
    {
3 ]- b! W" X* h' ^: ]/ k8 d  y      i++;      
) L+ c9 a7 q# A& s; n, `! R0 g) t      int k = 0;0 m4 o$ h, B0 Y% f! d7 J% }
      while (i < string_size && k < 10)
: w- A9 H1 p1 \7 ?: Y      {
" K, \0 z6 H$ e" V( G7 q5 F$ l        if (instruction[i] == 0 || instruction[i] == ' ')
4 D& j  [+ E5 }          break;  @, r/ v0 F( S) d/ T( f9 W: s
: V/ T1 E6 V2 I, \1 j: g9 D6 z+ M
        temp[k] = instruction[i];" v8 A7 W( K5 I% B1 i4 d
        i++;4 D/ ?4 j6 u( s, s3 n
        k++;
7 y& E5 R3 C, F1 Y/ Q2 Y      }9 R" \8 f( a* \2 T, \5 c$ a
      return strtod(temp, NULL);1 ~' \/ d% T4 m% }  g* r& Z
    }
" y: i' W  N; Z5 ^  }& L3 p: `, J8 i6 v

8 r3 |& j6 b* D" A( V7 g$ o  return 0;
* ]  c2 Q* W: K}" e/ n1 V+ s7 D2 c
) w- A1 S5 l( p" j
//look for the command if it exists.
0 Q: A% o  K5 Y$ e8 Dbool has_command(char key, char instruction[], int string_size)2 [$ W1 k, Y9 r  K
{. [& M* m& m% N! m. N( ]
  for (byte i=0; i<string_size; i++)
) {1 p- O9 u2 M" y  {9 }) [$ a& ~0 c) z/ u
    if (instruction[i] == key){
4 |. W7 G: P- B6 Y" G: I9 {, @/ f
8 A# Z9 \0 u" o1 q3 S2 V5 ?& c      return true;
! Z1 i5 W! T) O& y% i& |    }
: J% |; R$ r0 b6 Y  q2 k- Z7 Y6 n! y  }
0 }+ \7 d& I+ s" Z; ^7 a6 D8 J( O/ n( _. c% F; x
  return false;! C" s: l; b1 d2 F3 _; i2 g
}( F3 o+ ^9 [1 L7 s' v4 a9 u1 W

( G$ E! s( A* `3 y4 m+ u; [
5 y5 i$ l" ^9 f7 O3 T6 E) @' b) N: T# {. R1 R& S- i# l' I
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:55:37 | 显示全部楼层
+ ]3 x4 Q: |, i7 J
//init our variables
$ @5 o0 A8 k) Rlong max_delta;5 }7 z  ~# Y* ^/ Y% z  l1 d
long x_counter;
( u8 r8 @0 h: a' Llong y_counter;
3 |1 g1 f1 E3 {. ?2 wlong z_counter;  \' v1 t0 G# I% O( s
long u_counter;
( d2 \2 V/ H7 L- m! Clong x_pos = 0; // x position in terms of absoloute motor stepps
0 Q9 A7 w$ ]6 j  R- _long y_pos = 0; // y position in terms of absoloute motor stepps2 P5 M8 V4 ?" ?& s0 r
long z_pos = 0; // z position in terms of absoloute motor stepps
* y1 K# r/ U' \* u0 Tlong u_pos = 0; // U position in terms of absoloute motor stepps2 f- }2 T7 `* m' k$ ?5 L7 [

* U4 ^4 R6 g: t( v( r& _! |; Kbool x_can_step;
/ F4 a, e' P6 b- Ybool y_can_step;
6 c% U) o- {5 N) Ybool z_can_step;
; L$ _6 w3 I% s& @' {0 cbool u_can_step;6 e, e7 M" c2 x$ D  d, i: _6 A9 L
int milli_delay;
% s; N: V8 b1 m* I, G6 w7 `6 X
/ L# V( |  B. X, gvoid init_steppers()
# y$ r# ~/ U* a0 o{
* U% A) o, ?1 f& z3 L/ Z  [  //turn them off to start.' _3 s! f3 U1 F! [( r" {% G' Q# b3 _
  disable_steppers();
7 Z2 X0 S% k- |3 Z
1 [6 @& ?& k" D7 C2 \  X! @1 U  //init our points.
5 p. I! \; _# m2 ?' C& W  current_units.x = 0.0;
0 L" Y9 s# \6 K" e1 |4 ~0 I  current_units.y = 0.0;
+ O9 J( B. \# |% w" U. @( X3 b  current_units.z = 0.0;# x! {3 ?) h8 I0 M; y. q  d) Y: a) |. m
  current_units.u = 0.0;) `% ^2 S5 ~* n/ j! K3 _+ F
  target_units.x = 0.0;4 A/ r9 b3 r( X' q/ D
  target_units.y = 0.0;4 A9 W8 ]: v  X$ s0 p8 T, g) j; B3 `
  target_units.z = 0.0;, |, ~  W2 F$ z; e7 w
  target_units.u = 0.0;( X- |8 f+ U" w' b3 z6 X$ u
  
. {7 d4 c. N. t1 h3 y* F
5 h6 H3 M. m$ a8 t8 x" l  pinMode(X_STEP_PIN, OUTPUT);! t! g) j* ~/ r7 R' s- R
  pinMode(X_DIR_PIN, OUTPUT);
; Y2 @' ~3 x. C: [" B- N3 @  pinMode(X_ENABLE_PIN, OUTPUT);  Y$ X) r6 _9 L8 ]
  pinMode(X_MIN_PIN, INPUT);" c3 I+ M# i, n. D1 F) G5 s; c4 h
  pinMode(X_MAX_PIN, INPUT);
+ `9 ^  j# A9 l; j
6 ^: k4 V( z5 [! L  K* N9 b" l  pinMode(Y_STEP_PIN, OUTPUT);
, A6 ?1 }' L" e  pinMode(Y_DIR_PIN, OUTPUT);
2 W. V+ d/ ^( v# A* K  pinMode(Y_ENABLE_PIN, OUTPUT);7 \% i5 \- M4 y' O) L& l6 a& [
  pinMode(Y_MIN_PIN, INPUT);% i* ^8 P  _! Q; [
  pinMode(Y_MAX_PIN, INPUT);6 ?+ i9 _) [4 Y+ i- W1 `4 ~
7 a5 v* b3 L) _
  pinMode(Z_STEP_PIN, OUTPUT);
- B$ o; u+ h) [  pinMode(Z_DIR_PIN, OUTPUT);( [! e- l& q, m5 p! {
  pinMode(Z_ENABLE_PIN, OUTPUT);$ v0 F8 w8 k) l- G! C! t, V- U
  pinMode(Z_MIN_PIN, INPUT);
: A- s( |/ j* i9 z  pinMode(Z_MAX_PIN, INPUT);+ C/ q& x' K. W# g

2 M$ T# C! C; F5 v7 S2 x4 W3 M7 }8 T  pinMode(U_STEP_PIN, OUTPUT);, h( C' K) n/ j
  pinMode(U_DIR_PIN, OUTPUT);3 e& v- m4 g; n2 S' W
  pinMode(U_ENABLE_PIN, OUTPUT);8 Y6 e+ z: Q3 V
  pinMode(U_MIN_PIN, INPUT);
. ]/ v0 z6 L) p) c& p  pinMode(U_MAX_PIN, INPUT);( g+ p2 `+ S7 k  S6 q& M
  //figure our stuff.
+ N% [9 u& ]6 M) v+ K& d; X. d* A  calculate_deltas();% O' f9 V3 `' l% k6 t7 _% O
}2 Y; Q5 @7 }+ @/ F) G; B4 X! B

+ ?7 y2 E9 Z9 h: U* i. P7 qvoid dda_move(long micro_delay)& H- x5 |% \% M  D0 t
{
7 s  p: j" h8 s  //enable our steppers8 |2 w/ S5 W# O0 R" f, e1 B
  digitalWrite(X_ENABLE_PIN, HIGH);5 `9 {- B/ ?3 V" K; w0 @8 j
  digitalWrite(Y_ENABLE_PIN, HIGH);6 i9 H) g. {* P2 Y- k
  digitalWrite(Z_ENABLE_PIN, HIGH);, m4 l3 r1 @1 ?+ s8 ~2 u
  digitalWrite(U_ENABLE_PIN, HIGH);
+ K9 V9 [3 c, K# @, ?$ W  //figure out our deltas
$ w. L4 U$ z: H- [7 l! }- h0 `  max_delta = max(delta_steps.x, delta_steps.y);9 c) Y- @3 E/ u* f
  max_delta = max(delta_steps.z, max_delta);7 ?' f: m7 ~1 P6 q/ W
  max_delta = max(delta_steps.u, max_delta);
: j" p' L% N" g; ^  //init stuff.0 l9 l+ t* }% S1 e2 h: U- o
  long x_counter = -max_delta/2;; x* m5 x: Y$ j5 Y$ |% v
  long y_counter = -max_delta/2;
  W+ [9 r$ E7 M& o5 j; X  long z_counter = -max_delta/2;1 U3 D$ x, V$ O$ r8 m3 ^7 @
  long u_counter = -max_delta/2;& u2 C5 d  A& F3 Q7 h7 i" D
8 f5 _+ B* B  R5 X" I  @0 ^2 `
  //our step flags( ?6 S$ D! y* t
  bool x_can_step = 0;
1 u5 }' F2 z/ |2 P1 N. R  bool y_can_step = 0;
: R& c$ i* t9 a; r  bool z_can_step = 0;
; a, K8 {7 c7 z: p  bool u_can_step = 0;
; n0 W* Q- N% p1 G7 x3 Q8 M) x3 I& {; K; P( X; J6 [
  if (micro_delay >= 16383)$ g* B9 [# u# B3 ]" I# Q( i
    milli_delay = micro_delay / 1000;! V% }; h' I& ?5 u* w" z
  else
# L: K  I! ^. \5 Y6 x3 z    milli_delay = 0;' E+ k6 ]2 h8 K
: r3 N# I0 T# ^. Z7 h

% C3 E# M( i" O9 |  //do our DDA line!2 i, S, T$ j  y% v  _4 C
! U; b, ~1 O. C4 t. ?- o
  do
4 |. c4 }- i$ @; }$ S% y  {) @( R  G3 F& [6 K
    if(( digitalRead(BUTTON_SS)?analogRead(BUTTON_MAN)>>9==0:0) || stop_flag) break;/ y# W* u; N( y+ _2 E6 J: A
    x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
( |! T$ ~/ j* ]0 X' ?    y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
$ j, x2 Z1 w( K6 R7 Y- }/ o    z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);. I) \7 n( R% o/ g" G
    u_can_step = can_step(U_MIN_PIN, U_MAX_PIN, current_steps.u, target_steps.u, u_direction);% J) u% r: {5 ^
    if (x_can_step)
8 |% e; V# `) g: \+ r" Z8 }    {
% Q+ C% u' m# i7 F0 k      x_counter += delta_steps.x;5 S8 T8 o1 s3 z5 C* a

; l, g2 ~7 }" f/ m+ |) }7 H7 I      if (x_counter > 0)
- z9 D; d! q  o; g      {
7 c$ R. w- t$ {        do_step(X_STEP_PIN);- x% k% H6 b2 d7 j/ v
        x_counter -= max_delta;
. C2 c" W* `  s. p! S& D        if (x_direction)
& ^( K  s: ?5 W& g# ~  l         { current_steps.x++; x_pos++; }" b: H: h# A, |- `
          & d' s) r1 _7 W1 {, c
        else9 T: w( u7 t$ ]( v8 ]! S
          { current_steps.x--; x_pos--; }
6 y+ W' e. ^( [         ; L0 N: A5 l2 T( Y; y! C
      }
  [  z. g8 {" a7 @' W, U; y    }5 N+ S+ |3 }# E  B* }3 P7 E) ?* n
    if (y_can_step)) V! F1 o$ {! ]
    {8 C# o# d# ?7 Z% n5 W. S# i
      y_counter += delta_steps.y;
5 t/ o$ h9 m8 p; f8 l$ q0 I0 @5 l8 Z8 k/ n1 J& z
      if (y_counter > 0)
; }. [" j; D! g, c4 X2 F      {9 ^2 ]0 a4 T( ?; |/ R
        do_step(Y_STEP_PIN);
8 r" V, o- v; W- f        y_counter -= max_delta;
1 u' R, Z5 o0 `7 C& e2 d! b+ _/ G6 i" O2 S
        if (y_direction)( e0 x: v9 L# X% @' U
        { current_steps.y++; y_pos++; }
3 H+ w1 t1 l: q8 ^4 O" h, `) [0 u; z       ; Y7 @4 l5 r: ?# v: K1 _: `
        else
6 ^- V7 |$ n2 `2 m! n# C        { current_steps.y--; y_pos--; }
: z7 @8 X: t! u8 |; u        % U; M) m0 B1 m+ [4 y. K! Q6 N# \# q
      }
' W: [8 U# _! F# ^) r! K5 ~    }
# w9 Z1 @6 T) u3 a7 g0 M5 U$ [- i' x% c, r
    if (z_can_step)
) a9 m# z7 ]5 _) e  i    {/ E! A  `$ Y6 C+ P- s! _9 |+ I
      z_counter += delta_steps.z;
2 w. V' b- n- J$ Y9 G" c9 A8 g8 S0 V
      if (z_counter > 0)
) _7 J7 G( N, v! G      {
& l4 P6 V9 Q3 j# F7 J$ F        do_step(Z_STEP_PIN);2 m2 G! j+ a4 u2 w8 f
        z_counter -= max_delta;
; X( p4 a1 G! r4 J' C' d9 g) q! u) [# i9 ]; g: M& ^1 S3 K  f( q1 i
        if (z_direction)( O6 F1 i- H/ ^3 A2 {4 X6 W
        { current_steps.z++; z_pos++; }! o* i- y6 e& e7 L
        
- {% m. Y: U1 C! M        else: I# w3 \. R4 ]6 Z: s* U3 }8 @
        { current_steps.z--; z_pos--; }
1 h8 ?; f& L/ q0 j* R. y" W        + k% @" m6 U( G( u8 T
      }$ \7 l: n2 u* p, z6 `0 ]) B, a
    }+ q5 x5 A4 _" N+ L
   
. w4 V% g6 F6 q# b& t    if (u_can_step)
9 b8 q" k8 C) I( T) T) l% `    {
: _: F- r) u# N  e8 l5 k      u_counter += delta_steps.u;
4 i5 }7 ?0 }3 q( G3 k* i& T& J! D/ p
+ W& X7 [3 ~, G' ?      if (u_counter > 0)
$ |( w6 @! f( o8 O& X/ L' b: K      {
" g) b/ i2 E3 N2 w  [8 T  i        do_step(U_STEP_PIN);$ D$ z! S0 C3 Z' U& I5 F: H8 @9 P
        u_counter -= max_delta;
, M3 j; C) v$ o0 p9 l& l' _% O1 N" @* [
        if (u_direction)9 G) W* l, ]- n: E/ E: g. }
        { current_steps.u++; u_pos++; }' {# E3 r1 @/ E! v5 g
          # l( b& q2 L! `4 r  E1 Q( v- \
        else
. a% T6 A2 h) t) k# s; c          { current_steps.u--; u_pos--; }0 C! ]7 o+ r+ k% j$ [5 L  U
         0 s- {  A* ^3 ?6 z. }; F2 [
      }* a( V& [7 ?9 x: U
    }! h" P- S9 R2 B7 |, c
    //wait for next step.
3 Y2 Z; ~6 `, z- W1 Y& m! l, }    if (milli_delay > 0){7 }1 N( a; b' g! ]/ E: g
      //if (digitalRead(BUTTON_SP_EN)) SPEEN();# K; `: U2 l+ Y( U! @3 o
      delay(milli_delay);8 X) R, ?. P& \2 q
    }                3 F! A: x. U) O( D
    else{
! X3 @/ T) {8 C: }* r0 [* W      //if (digitalRead(BUTTON_SP_EN)) SPEEN();
/ z" j. h" O3 a; u      if(micro_delay>0)delayMicroseconds(micro_delay);, O7 ]! n. y0 L- [
    }
6 u+ G" n* s/ h0 ?3 s    //if(x_can_step%40 || y_can_step%40 || z_can_step%40);
+ _% l" z$ n- {! O" i6 P# W  }: @0 W& ?/ H" F/ M! b% ]: h! i0 r
  while (x_can_step || y_can_step || z_can_step || u_can_step);2 W4 m0 E6 n1 n0 Y

+ X# H) ?9 h0 E0 t/ T& O4 T
) K$ a3 c6 d1 j  k3 V  //set our points to be the same
9 F. S, k: Y$ n3 z5 O) Q  current_units.x = (float) x_pos / X_STEPS_PER_INCH;
3 o. [5 Y) I  D4 G8 B  current_units.y = (float) y_pos / Y_STEPS_PER_INCH;
- l; W% Z; f( d# q9 i! L  current_units.z = (float) z_pos / Z_STEPS_PER_INCH;# b: D" e, ^7 ~9 s& W9 @
  current_units.u = (float) u_pos / U_STEPS_PER_INCH;
4 I/ q: Q6 G% X( q* z3 F& ~  1 u/ ^- g$ f" b  l) f/ `4 R
  set_position(current_units.x, current_units.y, current_units.z, current_units.u );
9 {  K, ^  x# Z9 T8 R 6 H3 ~. W' `; r7 G
  long x_pos = 0; // x position in terms of absoloute motor stepps5 `4 g$ k( Z$ v; A
  long y_pos = 0; // y position in terms of absoloute motor stepps
+ J* H" H4 [0 s6 v+ p+ b  long z_pos = 0; // z position in terms of absoloute motor stepps
3 n" W3 k- U4 [1 i5 `4 y  long u_pos = 0; // u position in terms of absoloute motor stepps
2 i4 F8 i( I: N9 {  calculate_deltas();1 c  b! a* i% h% ?" e  w
  0 m8 {. k! h3 J6 n8 h) G4 F' j
}/ K5 C9 l5 g" G  R% k% J/ M
& J  T7 q/ _' b; `3 h
bool can_step(byte min_pin, byte max_pin, long current, long target, byte direction)0 z* j6 b$ q% b9 Z/ c
{5 ^, R( o$ v7 f0 j: m3 q9 I
  //stop us if we're on target2 l+ W% M+ z2 x# ~4 e1 g& \  O
  if (target == current)
9 \$ g1 Y$ V0 ~* s) {+ N8 y- V    return false;
7 g1 Q% K  y4 g) M. g  //stop us if we're at home and still going " p: v. {3 }" D' R7 a# W# g
  else if (read_switch(min_pin) && !direction)0 B: i: i( C7 \
    return false;
7 [" z3 q4 S3 ~# x/ d* X4 w7 ?7 j  //stop us if we're at max and still going4 G0 j% Q2 V/ R9 i6 B2 C/ A
  else if (read_switch(max_pin) && direction)9 v  h: n/ x7 v( _/ {% m6 o9 L
    return false;
+ a7 ?' ?. L, @3 A
0 o' ~: ?$ @9 `. ?  //default to being able to step
) E3 t7 S' b/ Y2 U' N7 W1 R  return true;
9 s# ^0 J: s: }4 i- C}. f: |. q0 r1 q
7 _" G+ U" Q3 `& g
void do_step(byte step_pin): S# D' Q$ t1 l9 W' @
{
+ l( ?6 S0 `! ~0 R% V1 ]  digitalWrite(step_pin, HIGH);
+ M, K# S9 f3 k, z  //delayMicroseconds(1);& ~$ O& S' n0 G- j. f; e( U, O
  digitalWrite(step_pin, LOW);
8 N% z- j4 Z* G% J+ Z  r/ `}
* e" V- A- q3 k/ ~1 j  W2 {, e1 M( k, H/ H8 a2 d" w! j7 w2 X
bool read_switch(byte pin)
$ s, g4 @! u8 u6 V2 L. T2 Y) ^{8 s& F$ C  d) d1 s
  //dual read as crude debounce9 C( t0 F3 k9 V/ P: c+ O
" X* Q; H3 t$ z- P! A1 E
  if ( SENSORS_INVERTING )  O( C# D9 K" x/ i2 N
    return !digitalRead(pin) && !digitalRead(pin);
9 t7 @; y! Y$ b# w+ \' U8 ~* w  else* t6 t1 ], ]! E( z9 u. M* e+ l
    return digitalRead(pin) && digitalRead(pin);* j# K" |% y! b; C( Y
}1 @! ?  m( ]6 a
) T" ^  {! L( q* I
long to_steps(float steps_per_unit, float units)
" n: X1 V. o- d6 c+ B. X{6 I& G7 w2 e- Y& H0 k
  return steps_per_unit * units;
$ _: Y8 _2 l3 T5 e}7 u1 U- q% I  ?  u5 ~
/ O+ y  |8 ^# I: b
void set_target(float x, float y, float z, float u)% X7 C& A7 w. C
{
" n; N6 W# d& [  target_units.x = x;
% q3 J( Z4 K% o" _. v9 Z  target_units.y = y;* V9 \: N5 Q5 D# X
  target_units.z = z;& o! p5 Q3 I$ T" B& m% i
  target_units.u = u;
) ]' |7 e- z9 Y  V& F  calculate_deltas();, i+ G7 I$ s9 q  K4 u1 O
}
$ {- L" V! I- _$ @+ }4 P# @. A# k' j# q/ n
void set_position(float x, float y, float z, float u)
  W8 R* Y! V9 x: s6 k3 l{
4 w# t  Y  i- q. w; ~& N  current_units.x = x;
5 w7 G0 M) J' I, N  current_units.y = y;" U% H1 }; {2 H7 i9 T! o" s
  current_units.z = z;
4 e# n, ]: }% [( h% l" d) b: y! x/ Q  current_units.u = u;
3 F6 p. c, b6 ~" m' P, v, A  calculate_deltas();
' P+ f5 ^3 [& z" N0 t: F. t5 e}& s9 j* l: Z/ B% {

5 y( H2 V0 w7 S( `+ ~) z+ p1 tvoid calculate_deltas()9 \- w" R. J* ]3 _1 `
{
6 z. W3 E- U& I5 M3 z  //figure our deltas.
  \# v0 m+ t) c# I- P; A( }2 F- G: W  delta_units.x = (target_units.x >= current_units.x) ? (target_units.x - current_units.x) : (current_units.x - target_units.x);
7 ?+ b: C  E) V  delta_units.y = (target_units.y >= current_units.y) ? (target_units.y - current_units.y) : (current_units.y - target_units.y);$ e; S7 F! T! X2 K  w% G& l( a
  delta_units.z = (target_units.z >= current_units.z) ? (target_units.z - current_units.z) : (current_units.z - target_units.z);+ R# T' o; }% D' ?6 n
  delta_units.u = (target_units.u >= current_units.u) ? (target_units.u - current_units.u) : (current_units.u - target_units.u);
# l* ^' @! t( ]
7 M$ f& O# U. Y/ t8 [1 L  //set our steps current, target, and delta9 ?, m/ Q9 o! l% s( Q1 N  E0 x
  current_steps.x = to_steps(x_units, current_units.x);
6 a6 `" Y4 x# ?( b5 M  current_steps.y = to_steps(y_units, current_units.y);- `, j6 t% o& y- U, x8 ^1 |+ q
  current_steps.z = to_steps(z_units, current_units.z);
$ Z9 u* Z2 V* ^8 C, R2 f  @  current_steps.u = to_steps(u_units, current_units.u);/ X5 z3 [0 @/ j* A8 o% z% y
- R; D1 y* Z3 P( y
  target_steps.x = to_steps(x_units, target_units.x);
2 K2 e, s2 u  G- x  target_steps.y = to_steps(y_units, target_units.y);& @: |  O" L7 g- a* S7 z# G
  target_steps.z = to_steps(z_units, target_units.z);2 c0 g# Q; u. S
  target_steps.u = to_steps(u_units, target_units.u);
. }/ M+ n! k& D/ X
" H$ L& G4 r* f3 [- ?  delta_steps.x = (target_steps.x >= current_steps.x) ? (target_steps.x - current_steps.x) : (current_steps.x - target_steps.x);: D& R. W; @' ]8 n
  delta_steps.y = (target_steps.y >= current_steps.y) ? (target_steps.y - current_steps.y) : (current_steps.y - target_steps.y);
: E4 b4 G7 s; v0 q  delta_steps.z = (target_steps.z >= current_steps.z) ? (target_steps.z - current_steps.z) : (current_steps.z - target_steps.z);
. K& u. M/ R% y6 E6 u9 q  delta_steps.u = (target_steps.u >= current_steps.u) ? (target_steps.u - current_steps.u) : (current_steps.u - target_steps.u);
% [( m9 V* q/ @: t6 r! p7 r+ l) d3 F* a. Z* T3 s0 H
2 \/ A- O) w8 v6 ?$ _% H1 h

2 t3 c  d% ~6 h$ M0 t6 D6 ]+ m  //what is our direction
: y9 P; U! q- @6 R  K/ a  x_direction = (target_units.x >= current_units.x);1 L2 ?! W" N3 y- Q- p$ t3 L
  y_direction = (target_units.y >= current_units.y);/ C. T. n" j. W& J. T
  z_direction = (target_units.z >= current_units.z);
0 h" l4 A2 c6 ~- C  z  C  H9 y% d  u_direction = (target_units.u >= current_units.u);
- b1 u" h& R' ^) ^+ O
+ U4 Y& C- C: s( D  //set our direction pins as well4 q, g( `" f) k4 ?8 O. p2 @+ _
  digitalWrite(X_DIR_PIN,x_direction);
4 `$ j- a2 j+ Q. l6 M- H  digitalWrite(Y_DIR_PIN,y_direction);
3 }- K1 |8 `  R* N  digitalWrite(Z_DIR_PIN,z_direction);9 ~0 I% V3 B: ~% `# }
  digitalWrite(U_DIR_PIN,u_direction); - Y2 _2 i3 L6 u
& r  y/ y4 n' G0 J, }
  //绘制LCD
1 L5 U& {9 B9 Q; e. L% Y$ n  LCD_DRAW();1 {# ]: Z0 @2 a7 Z' `; e- ]
$ R: m( r6 b& |( \" }, O5 |+ Q% v; p
}. A! k% d3 c1 G2 D2 y# J  {
* e$ H+ o8 \  \7 n

$ A; s7 Z5 e. h8 O% O6 Jlong calculate_feedrate_delay(float feedrate)3 r" x6 |8 R- b! M  n: Q
{
$ _9 r+ ?& L  T/ A0 Q# C' g' V9 b9 O9 m& B  //how long is our line length?8 M% j( c' a" g7 @' m( q9 j5 y
  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);
. a, a1 J% |2 p, h3 P4 v  long master_steps = 0;  W8 t6 a0 G6 |% }4 u8 g

- X2 n9 W4 W$ s. y1 z# D# j+ r* ?6 umaster_steps=(delta_steps.x > delta_steps.y)?delta_steps.x:delta_steps.y;
/ x: u/ ], K9 k9 w2 K" s7 B: g7 Vmaster_steps=(delta_steps.z>master_steps)?delta_steps.z:master_steps;
* n) y: M- g4 [3 umaster_steps=(delta_steps.u>master_steps)?delta_steps.u:master_steps;; ]5 ?7 g% [. Y" \! B9 v; g3 j0 \3 I

$ I+ }* A! b" K6 ^7 Z. _3 w
3 `- s1 s& `% _$ c* u1 ^* w
7 _5 F4 {0 n3 G) W% z/ M# W * e0 C3 P* }. }2 }& N9 R5 j* F! y
  //calculate delay between steps in microseconds.  this is sort of tricky, but not too bad.; k0 a; f$ J+ S  n  Y. ^* m
  //the formula has been condensed to save space.  here it is in english:" s9 M5 ~. }- Z( [- V- ^4 a1 f
  // distance / feedrate * 60000000.0 = move duration in microseconds, K' @$ d6 }: @' D% Q# r
  // move duration / master_steps = time between steps for master axis.: m2 y2 ^0 n' H2 z
return ((distance * 6000000.0) / feedrate) / master_steps;- M- o( c+ R3 L" B
5 Y& m3 [5 C* h8 G
}
$ g4 G; ]% U& z/ p9 y' ^$ A/ P* I+ U; F0 M& a- g) o# W' M( w
long getMaxSpeed()" N" x- x0 v& g9 X
{
1 E0 m1 j& e4 L' L7 Z if (delta_steps.z > 0 || delta_steps.u > 0 )
" m, b& _( w: K% T. @& f2 l+ i7 X    return calculate_feedrate_delay(FAST_Z_FEEDRATE);8 m" Y9 |) b7 `
  else4 W: @/ U2 E3 ~  ~3 ?- k! [
    return calculate_feedrate_delay(FAST_XY_FEEDRATE);
" e/ D; J5 e7 w# L}. w; i& G2 l; g4 i4 D
* O3 F* }. @9 l6 x
void disable_steppers(). r0 @# R5 ^6 y  l1 G- ]
{) c4 e2 Z; p- f9 A7 A8 Y+ I
  //enable our steppers2 [" O0 ^# {2 _+ S$ v" a' o$ R4 {
  digitalWrite(X_ENABLE_PIN, LOW);, \0 ]3 P( i" A8 a! [
  digitalWrite(Y_ENABLE_PIN, LOW);( O' E8 U7 W$ ]( L1 c
  digitalWrite(Z_ENABLE_PIN, LOW);% b& H! {- z) @' {; S) J7 l
  digitalWrite(U_ENABLE_PIN, LOW);
7 G* [/ _' k* f}1 U) f* C3 N, k- X6 J* ~

3 J3 D9 p. M, h  Y+ X5 B( R6 B
$ `4 v4 x1 _: ~0 J//绘制LCD
' g/ V; s; N. ]- m//unsigned int DRAWCount=0; & T- V& n" q1 ^& q8 i: k* w- z
void LCD_DRAW()
; F1 k, }/ S, Q/ F3 B, v4 d$ Q8 R{
. |4 @$ F# W1 p1 {, z7 i/ F    lcd.clear(); 6 N/ ]+ r+ i( R6 z2 W) |/ W! F. v
    lcd.setCursor(0, 0);
4 R3 d) y; y1 y( {) Q    lcd.print("    X=");) ?% r3 m3 Z) j1 X
    lcd.print(current_units.x);
5 q& f7 K  H4 D4 z8 S8 }    lcd.setCursor(0, 1);$ H6 s/ Y" A$ {; b. \
    lcd.print("    Y=");) F* {& p6 O' u4 E
    lcd.print(current_units.y);8 f8 t. C+ h3 e8 Z" z
    lcd.setCursor(0, 2);
( [- N! O9 O% s( j" n% w. }+ A    lcd.print("Z=");
. I, a1 F, Z! u9 F    lcd.print(current_units.z);2 F. D$ i( E, [6 l1 j* F3 i' e
    lcd.setCursor(0, 3);% g, k0 ?9 h) z) N
    lcd.print("U=");) U+ i2 V4 k" ?, M- z1 G  l' d
    lcd.print(current_units.u);
+ l3 E# Z& M5 Q8 I  }- H3 z6 C! B  C+ D9 c8 r
// else if (DRAWCount>=30)DRAWCount=0;
+ Q7 P. ?7 E% X$ M% C# f. o//  else DRAWCount++;3 ?% B4 b6 q" X: z, ]& z) `
void SPEEN()+ Y/ ?( a) Z; [
{* ]( _6 l* p% G2 \
   delayMicroseconds(analogRead(SPEEN_CTL)+1);
* g0 Q& ]+ K9 v }# J# M9 m; x' W: }5 n& O
/ J! G, _1 h, p; q! l' B
2 E4 q# r2 c0 A
//delayMicroseconds(analogRead(SPEEN_CTL)+1);
! i+ R7 Q" W9 ?, \" y8 W //if (digitalRead(BUTTON_SP_EN)) SPEEN();/ ]$ A- g  t* `
! `( G( D& b9 u' V1 l$ t9 l
; `; k+ P8 F3 r4 w* S$ x& y8 g
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:58:04 | 显示全部楼层
基本原代码的大概是这样,我是断断续续捣鼓的,玩到最后版的,后面我会找出来先测试下过,再贴上:)
回复 支持 反对

使用道具 举报

发表于 2014-5-10 10:48:17 | 显示全部楼层
请楼主发一份给我,感谢!178354773@qq.com
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册会员

本版积分规则

小黑屋|手机版|Archiver|机械社区 ( 京ICP备10217105号-1,京ICP证050210号,浙公网安备33038202004372号 )

GMT+8, 2025-2-19 06:23 , Processed in 0.086358 second(s), 15 queries , Gzip On.

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

快速回复 返回顶部 返回列表