机械社区

 找回密码
 注册会员

QQ登录

只需一步,快速开始

搜索
查看: 23937|回复: 14

运动控制入门篇GCode_Interpreter

[复制链接]
发表于 2014-5-10 09:39:53 | 显示全部楼层 |阅读模式
本帖最后由 xmdesign 于 2014-5-10 09:42 编辑
. \0 u- ~6 U* F& a  T! Y  ?0 J7 F" L& r4 U5 a" h6 s: N
首先声明( G' M1 h1 D/ @5 e4 ]) ~
我不是专家,业余捣鼓2 N6 w1 A4 \6 k) S
源代码首先来自网络
; I- u) R2 O# E% [- a- z: S) j开发平台Arduino及Maple# [' A6 t  A0 o$ `, b/ x& u9 X
http://www.arduino.cc/  L1 x5 o1 U- M+ s
http://leaflabs.com/devices/
2 Z1 a6 ]6 \0 K/ R9 Q3 {国内活跃社区
7 H" A4 _( W7 C2 f2 Mhttp://www.geek-workshop.com/forum.php
8 @- g& d4 x  a' l, {0 s4 [* s! C* L
1 `+ z3 M6 Q: u' v, A竟然缺少积累,首先寻找最大的开放式开源平台,这样可以积累全球范围的创客和DIY者的脑力活动经验/ W4 c" Y6 ?; v2 l9 e
来入门了解熟悉思路架构等/ g4 }1 V# ~+ T+ T
0 U+ `( j8 l1 b6 C% J6 y5 N: P6 M
我只捣鼓了下8位机Arduino 移植到32位机Maple 方面一点点事情,' _$ F" W/ v) U
许多功能还木完成,不过作为低档次得应用可能还有可能4 R* H& r4 }- [1 G' I

5 m* n3 |5 P7 A3 w$ L3 l3 k2 z  I我自己也是个半桶水,再乐意玩的起码自学能力还是要有点吧+ v& p) Y% l, r: Y- \7 L

0 ~) J, C, v1 n) f$ ^, ?) T7 j拒绝 所有的求0 n$ s# \4 f( ?
求人不如求自己 不然木玩) r5 @3 k; F- T' ~% v* i# \8 K6 T

: {% A) e* h" D% g高手绕道 谢谢!8 m$ U, ]  ^. z# D5 R
+ J. U- B4 v. x! Q( q# f$ B( }+ h
回复

使用道具 举报

 楼主| 发表于 2014-5-10 09:48:37 | 显示全部楼层
本帖最后由 xmdesign 于 2014-5-10 09:52 编辑
' M: X6 X: Y/ |' c( X  p3 f& D# h& t& z; j& L2 d2 X) z
GCode_Interpreter是比较容易懂的,木那些寄存器等虾米开始不容易懂的东东
; c; R  U0 c' q4 e贴代码先% E3 T' a! b, a  i0 s# k
直接Maple的,某宝许多超便宜哈
7 G- j3 a1 [% T) y+ d7 @+ o  J晕,我怎么上不了RAR?
3 p; N% y* @0 l想玩的留下 e妹吧
; t, r- I* G" H# U第一个妹麻烦传第二个妹哈
$ g$ Y3 f) z9 X! M; s, @我平常杂事多
# J8 L$ U. q5 a2 A0 o谁放网盘上再,麻烦开放下
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:52:53 | 显示全部楼层
// Arduino G-code Interpreter for Rep Rap
/ j& s- E) \4 R- A! I% F// v1.0 by Mike Ellery - initial software (mellery@gmail.com)
& ~1 I9 H6 i/ J// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)
( h6 K4 n2 ?8 I: B+ G// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com). t# z4 L1 U0 d6 l( G8 i+ o
// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
3 {' p; y* \4 \$ {( z$ B1 y$ x+ v8 c( J' M& H- J. C: Y
// Arduino G-code Interpreter for Macro / Micro photography1 O9 F7 E! x) i4 N) s
// v1.4 by Gene Cooper - modified and setup controls for macro / micro photography (gene@fourchambers.org)( V  o3 v5 ^, S3 o8 d7 X
//modified by YaoHan China 2010.2.15  D7 D" u+ F" {& F4 H
//modified by JiWei China 2010.2.22 (jwdesign@163.com,QQ:75990175)
( s3 w/ ]! m& L#include <stdlib.h>
- |( q, v  B1 s- u! `#include <LiquidCrystal.h>3 A6 B0 N' f8 d: z

8 e3 g6 E1 j5 E" T//启动(高电平有效)/暂停(低电平有效)  默认高电平6 k" @" }" U" ^% Q# D8 P
#define BUTTON_CTL 352 h# u; c1 C; A6 N! r

: h  _0 ~( n+ l$ V' b+ Q; R//点动模式键(高电平有效)  默认低电平
5 A& ^) ^1 H' g5 \#define BUTTON_SS 361 c6 o6 J( P6 z
( i! D- b/ S. n2 @" R$ V9 A
//点动键* _9 [9 C- r; M) {" c, C
#define BUTTON_MAN 15
$ {0 C  k; k! h4 ]" P. \
, y8 [3 H0 _, ~//电位器速度控制
' C4 o0 i1 Y- r2 B6 [#define SPEEN_CTL 16; e& D" Q  E5 A9 w# u0 [
) T* d! l  m* ?; ^. Y
//手动调速使能' w& t9 B0 u  T( g2 c0 c$ ?" j5 @
#define BUTTON_SP_EN 17$ e8 s2 b. ?& r$ B0 L9 F
& B& h9 e. K& l9 O7 X& g0 C
//LCD 类型 1604或1602) X- ?7 e+ M! {( T6 M' F
#define LCD_TYPE 16043 V* Q6 G8 O# Y+ q4 _
//LCD 引脚定义6 u$ P. m1 @+ P) `
#define LCD_RS 23: |8 N/ B* ~' i) w+ D8 U1 y
#define LCD_EN 24
. U; g( j' M0 i, e#define LCD_D4 255 S% A/ a: e& J; z9 t
#define LCD_D5 26
0 G' ?) N6 I: a#define LCD_D6 27* K* U4 B1 P% [/ a! b
#define LCD_D7 28' V) r( @) O0 l

( t+ T6 m7 h9 t//命令字符串
1 s0 @$ s5 M& |0 T; b8 Y#define COMMAND_SIZE 128" q+ ~# l4 i  Z$ p; E8 |, F& p5 w7 G8 t
char word_old[COMMAND_SIZE];( R4 P3 _" y$ P" X. M  |: ]2 Z
byte serial_count=0;! v. H* |0 r- k# A4 p$ M, L
int no_data = 0;0 i9 @5 M' e2 M- e' E. N  x6 v3 D
//LCD 引脚链接 配置
9 g" z; L' K- @; Q2 g/ KLiquidCrystal lcd(LCD_RS,LCD_EN,LCD_D4,LCD_D5,LCD_D6,LCD_D7);
7 O, Y1 f; L& U3 c//停止控制,高电平有效
- m3 X- o' m* z4 E: e#define BUTTON_STOP 30' q$ P' e  i" v' m4 a
int stop_flag=0;1 @4 @3 K. o( K$ F: S! q- e3 M0 o4 z

+ j' a# w" H4 g7 n2 [3 ~, R) U, |//暂停
& |' }7 o  |' n! I0 evoid pause(){
  S5 Y8 p  e: k9 y# a1 a3 g4 [  while(!digitalRead(BUTTON_CTL));, b- t' C7 s% B6 H
}3 X8 c( A3 Y3 ]# s4 n) I" F5 ^& {
) x' w& P5 r6 C
void stopper(){+ T, \$ d0 w, K# r" ]
  delayMicroseconds(10);! o7 p; R9 b* r: F) e
  stop_flag = digitalRead(BUTTON_STOP);
2 ^( z% i$ ]* i+ [" z) ~}
# x5 s( u8 z; ]' @0 \5 H6 n9 h2 o2 g5 S; ~9 V% R" T! P
void setup()
, }+ E0 U! I' m0 A3 y% s! k{$ K( {: ~+ F5 G5 N) C5 |
  //show start5 U0 V7 |9 l& v% b+ P- E
  SerialUSB.println("start");3 r% ~4 B  P9 T

! G+ G  [& Y$ D6 n  //启动lcd# c; x& f& l4 J2 W2 Z" k
    lcd.begin(16,4);5 y% s7 D  G" z* G/ z* Q1 N- k
    lcd.setCursor(0, 0);
% a% v' N; g3 Z    lcd.print("hello, world!");0 G, w, A1 j: L6 F5 O& U2 R

) y) h0 n  L/ F  //初始化控制引脚及引脚功能
- |  l1 N1 M- H5 o6 h1 R  pinMode(BUTTON_CTL,INPUT_PULLUP);) Y; A4 p2 \! M5 m( s
  pinMode(BUTTON_SS,INPUT_PULLDOWN);
( [) E. [+ o) W* W  U% K& R- N' `  pinMode(BUTTON_STOP,INPUT_PULLDOWN);
/ g3 g  [2 p/ R6 ~# w2 f  pinMode(BUTTON_MAN,INPUT_ANALOG);5 C$ S% D+ }0 H5 P' |

+ O3 E- h0 z7 V7 R2 I  //控制引脚的中断
: o$ m: f/ z  ^* d  attachInterrupt(BUTTON_CTL,pause,FALLING);//暂停的中断' T" f. L* O6 O# q
  attachInterrupt(BUTTON_STOP,stopper,CHANGE);" D# H- U" u8 s- ?( L' [

/ o. H1 k; T0 K3 p/ y; \$ T  //other init8 v2 @- z& z+ ~1 {
  init_process_string();- h0 J( Q7 A8 c
  init_steppers();3 B+ l  t( |/ c1 u
  init_camera();: m. Q* d: K1 p6 t% D: d% }
6 o! ^' w) D4 r0 t5 t( P* u/ b7 C
}4 X. \" b( O+ ~( X; C

% D) l1 P# w1 u: O; kvoid loop()) W, N5 W$ L. u4 n5 D7 v
{
  x) o2 N) F1 C  char c;
! F: o# h% I2 w' g9 ]/ Q
* h3 J9 [5 [' F% v0 I  //读取输入的字符
5 ~% z3 Z4 j0 m7 c- I4 n. c" `  if ((SerialUSB.available() > 0) && (!stop_flag))9 X2 Q1 G# [/ {) M# a, P6 ?
  {3 `$ b( N; F: [5 z) A! H. D1 j5 r
    c = SerialUSB.read();7 E) L7 F1 m% O/ M) v* n
    no_data = 0;' g% W* ]9 l: l7 m
7 f+ w+ R/ j5 u% `5 N) E( W+ A# }3 B4 y8 {
    //换行符代表一个命令的结束
1 E5 C4 L9 [/ ]6 |) x& E, Y2 m    if (c != '\n')
% `* U8 A+ u8 T- [& D* g    {$ R$ t  S' F) }7 J7 {8 @+ D% ~1 D) }
      word_old[serial_count] = c;+ }3 T2 s. I7 y- I9 s
      serial_count++;
# C) X7 B# O! x$ n! [6 B. V1 }& }
. \6 o8 H0 m% j+ L+ [0 r2 {    }" a7 Z& A  `) F$ h2 k- C
  }1 P4 d4 y! {. M' N8 v' p% s
  //标记没有数据输入$ y! K- v$ E( U' z1 j
  else2 q+ {. Y/ u1 _: ?' I5 C
  {9 r. j, }% ]. P8 h
    no_data++;) |* r7 |# a! V; [6 V
    delayMicroseconds(100);
) n! m/ y; h6 F3 k+ G3 M4 X  }
; q( b, _& p# C+ d" y) d
! S+ {* W. }1 F1 q( K" A  //if theres a pause or we got a real command, do it
& H4 S7 ]7 U1 n0 b, R  if ((serial_count && (c == '\n' || no_data > 100)) && (!stop_flag))
4 ?# s' u6 S# S  {
% t1 L" C# L* f  N- x3 Z' v) H- C% j' m# x6 A
    //处理命令; O" k4 k" r1 P8 A4 N& B3 ^
    process_string(word_old, serial_count);2 p  h2 T% N7 \
" q9 J: h% f' Z2 Y3 Z5 f( S5 T
    //清除命令
( Z9 A. n4 N! r/ f8 v( o& \    init_process_string();
- G, n% [6 U0 e8 d1 @% C  }
5 a( l1 T! [( {' S0 B
) F1 F" u& N) s! F8 f  //如果没有数据关闭电机! k9 C8 y+ c+ Z  R! g
  if (no_data > 1000)
$ `: Q, @& z* Y" Q    disable_steppers();  ]8 n+ R  l) F* X4 H) x
: ~5 x; K& ]' y3 }
  //如果ss键按下进入点动模式
% m2 W# `" s9 R: z  //if(digitalRead(BUTTON_SS)) ss();7 l' C7 W, r$ m6 }
  run_a();9 l- L/ B+ q% O6 Q; Q
}8 V! A# h9 S  G+ o  Z1 i& ~! D
+ B$ d$ q  j8 \9 P0 L
//点动模式
' ~, L  G7 L& I4 svoid ss(){
! x, }0 w( |1 ^6 w8 ^$ d
$ Q/ M: _) O3 e# R' \3 K, ?  delay(1);; C' _' \  r5 l6 c; E& C0 r
  if(!digitalRead(BUTTON_SS))return;% K5 `  |8 F1 t3 `1 K. S- Q
  if(!digitalRead(BUTTON_SS))return;
" @" t, ?6 O  w5 h1 j: N. e  //init_process_string();
7 R! _+ L6 z5 `  //init_steppers();, K0 w" _+ @: T
  //init_camera();
, X8 p8 k! ]6 J. f
6 ]+ h& ~/ f$ P0 {9 T$ g. C. f. ^ // SerialUSB.println("Step By Step Mode");
- [9 n$ i0 _' i+ c, F, E  B: [//#if (LCD_TYPE == 1604 )
/ w( g, G( ~( Y2 i5 {4 i9 Z  //当LCD为1604时显示要处理的命令
) Y3 l& Y, }- [6 Z: J  u, g; u1 H   lcd.setCursor(0, 0);( O* [4 `/ d* [, Y- ?0 }
   lcd.print("Step By Step Mode");
' U) C8 n, P- p% N, e% A//#endif
3 g  J  o7 F# f; P  process_string("G1 F5000",8);, L! m) y; f) g  K7 N7 C8 R+ i, L
  process_string("G91",8);7 Y3 o1 v; U2 n
  //init_process_string();
9 S7 Z0 z" A  Q6 P4 N
; h$ [! E+ l# Y/ ?- F( \4 C5 h  while(digitalRead(BUTTON_SS)){* C2 \, d) I+ x# W* `) L
    int i=0;
1 L7 ]( g9 i) G% i' G    i=analogRead(BUTTON_MAN)>>9;
7 `. [+ c. V5 j# U' P    //if (i==0){break;}
7 |: R  E  d: O" [3 y5 m   //SerialUSB.println(i);
& F& o* c) @- o( F5 h* ~' l. m   //delay(1000);   & x& f* V- m1 u2 r% V5 a

2 c( D; C# E3 C8 O% l* ~   if(i==2){% E( j1 y5 H8 L5 D
      set_target(1000.0, 0.0, 0.0, 0.0);3 L; B+ V/ ^. a5 Q4 A+ X  T
      dda_move(getMaxSpeed());
9 ^+ @: J9 {0 b3 t. R" D      if(stop_flag) return;
+ D9 }9 k' H$ O" o9 B! `! x      //process_string("X0.01",5);' A' p& \4 Q  w% @: e
      //init_process_string();
0 f! }, m! E; M/ a9 Y2 q    }
1 E3 z- ]- m7 ^$ D' u2 n    else ; x( ^) e. T9 N' g
    if(i==5){4 K; C) l7 S# Q, K4 F& U) k
      set_target(-1000.0, 0.0, 0.0, 0.0);( H$ j9 g) `' x+ R
      dda_move(getMaxSpeed());+ v7 X5 l) m4 P  C
      if(stop_flag) return;
: U0 L( L# _4 Y) J# U! Y2 c      //process_string("X-0.01",6);
4 R/ v! S! A# r      //init_process_string();
% G& ^. a6 W4 w' d    } " I" K" n' f7 A% m: T
     
4 j7 A' q% E# K/ ^8 d% L3 Q9 J$ M/ J1 S3 h! P7 ]3 p
  }
+ _1 u) M0 }6 |6 E3 ?/ ^
+ O+ \$ j5 }& m# T8 |0 n5 {! u4 U  //init_steppers();
' f* v6 U/ u2 o8 d! N! `$ }  //init_camera();3 N# M7 o" Y: T* }
   // SerialUSB.println("Return To Normal Mode");. g6 }0 k2 G9 w" v! F" C
     process_string("G1",8);
! f( j9 [  T. i  K" U$ G: G  // process_string("G91",8);$ [9 x% q: e2 U# N' p
    init_process_string();0 p# G, n9 e5 Y( }
//#if (LCD_TYPE == 1604 )
6 c' m2 t7 n  M  //当LCD为1604时显示要处理的命令' T5 _1 M7 w: z. K( a1 t
  // lcd.setCursor(0, 4);+ {5 G5 i- B8 K& r% U( y
  // lcd.print("Return To Normal Mode");
) o: y" h% `" x. D; j//#endif
, W# P( _. t- Q1 @}
* v- A/ O2 v6 m$ j
0 q2 I1 p9 q! [, V7 r$ Kvoid run_a()
) k$ N) M! M) h8 Q5 a5 L% O8 A{
! B$ ~! b2 }/ c- {- d  //delay(1);
. y/ v$ o: J5 q. ~4 S& R4 Q) [  //if(digitalRead(14)== HIGH)return;5 D( f, {& @( \& V! v
  //if(digitalRead(14)== HIGH)return;
3 a: P  G8 x& \$ V3 S  //process_string("G1 F2000",8);* W; g. a- f2 D" c
//process_string("G92",8);  j9 D! \, D$ w. J* S
process_string("G90",8);1 f6 F# U( N( U7 I* E
process_string("X120 F10",5);
, u9 g/ e3 S% e% N; [ process_string("G4P2000",8);4 n, A4 ?" ~. L) B
process_string("X10 F10",5);
- G7 S7 m/ V$ Q! r( U// process_string("M101",8);
; o; T( a) V, c- l// process_string("X-50",5);0 o$ P; ?! W$ |# B& P* X1 C
dda_move(getMaxSpeed());
2 k% l$ d1 J$ F" q init_process_string();1 R3 d+ s9 L' J0 H* P/ p
+ B1 k6 ~# x' z6 E
}/ m" O9 {! K, ?7 k. _, g( f
回复 支持 反对

使用道具 举报

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

1 v& L7 V4 e4 ~- H: L
- b& B3 n4 P+ q- w  Q7 S// 定义机器参数
! I4 ]8 i' x6 Y3 N#define X_STEPS_PER_INCH 4004 E' W# a( J+ l7 D7 y
#define X_STEPS_PER_MM   16.0. |1 ^3 {( t6 N# `7 J
#define X_MOTOR_STEPS    200
/ j! K7 Z3 i- D, I# x# b0 w; H7 p! F5 g7 {4 V
#define Y_STEPS_PER_INCH 400.0
" @& k2 k" U7 U% H/ q4 P#define Y_STEPS_PER_MM   16.0. b$ O2 G/ g/ t  g/ n! X
#define Y_MOTOR_STEPS    200
% k" G& z: Y: m( e" J
5 u0 [* i$ h: G4 R( v& z4 K) n#define Z_STEPS_PER_INCH 400.0
4 M1 j! [" \$ x! P6 V8 M#define Z_STEPS_PER_MM   16.0) U' P+ q6 Y* e& s
#define Z_MOTOR_STEPS    200
# {% g3 _, O4 j9 U8 C0 r
, s( ]( E8 y+ ]! Z#define U_STEPS_PER_INCH 400.0+ ~# ?. ]/ [/ Q* |( ~
#define U_STEPS_PER_MM   16.0. s4 m+ V! y& B5 m$ V( {
#define U_MOTOR_STEPS    200+ q* s  L# S, l

- ], \# o/ H* N: i+ N4 C0 Z& l1 s$ m//最大进给率
* l- W: r( z$ L) o- A% a# i- J+ O#define FAST_XY_FEEDRATE 1500.0
3 g+ J/ o9 @/ K( V" k#define FAST_Z_FEEDRATE  1500.0
% x4 m' c2 }/ U6 w  s#define FAST_U_FEEDRATE  1500.0
% J4 Z- W  K, R9 p) y// Units in curve section
& c0 i9 K1 j+ F1 s1 ]! {( k6 H#define CURVE_SECTION_INCHES 0.019685. X! a0 O9 N5 }4 ?: j1 j
#define CURVE_SECTION_MM 0.5
7 z+ ?- O& r# V+ M7 U9 \+ W6 A; E8 o  q7 M( j
4 [" G" d" j( Z8 S% g0 C8 @
// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)6 J7 P' W/ R# D- E/ Q
// RepRap opto endstops are *not* inverting.; Y# G" q' p" Y+ M$ V- M
#define SENSORS_INVERTING 1
& c! ?$ J7 J0 D
6 a# m1 T: V, C) R3 E/ B/****************************************************************************************
$ u; a7 y# t; X  e8 a) v" u * digital i/o pin assignment
+ K3 L; d2 `6 Q$ g% @6 B! E+ ~( J *0 h/ Y8 l3 G! R, y9 y0 e4 |' M; e6 u
* this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5
( G: _; G3 ~: f# ^& K* i. M ****************************************************************************************/
  n# _' o. j! v3 \$ H8 S0 `, g* _: n5 P) W5 I. M- f
//camera shutter and control pins( h& c& L" W' l8 F$ ?% ~
#define CAM_SHUTTER_PIN1 29& p# ]& d' e' _( S, `, @& A2 b% o
#define CAM_SHUTTER_PIN2 30+ f5 f9 N4 j8 |
//#define CAM_AUX_PIN1 31 // analog 0' ]' z2 ]3 \. e+ a/ \; n- t
//#define CAM_AUX_PIN2 32 // analog 1% z: `1 J* K0 q
//#define CAM_AUX_PIN3 33 // analog 2+ y% Y+ Q, M( E" E  a
//#define CAM_AUX_PIN4 34 // analog 3
# R+ G) Q, G9 h7 X& v& L1 K* ]8 D8 T6 M* k8 {5 ?/ g
// stepper driver pins
$ [- q& P; l; q& ]- a* T9 ]" ?#define X_STEP_PIN 7
% J0 V5 \" ?  C+ ?#define X_DIR_PIN 8
: s  X# U3 x0 h) y3 B9 ^0 z#define X_ENABLE_PIN 19
( _0 z5 U& Z5 P: L0 b& B$ |, Y) g8 L& t. }
#define Y_STEP_PIN 94 y8 |0 B4 q# l& Z' _+ @, E
#define Y_DIR_PIN 10+ u: P, s' t7 ]$ S- ?) ?6 w/ z$ ~/ E
#define Y_ENABLE_PIN 195 P/ u% |, z* D* o/ K9 M

1 s& g6 C5 A" V/ A#define Z_STEP_PIN 11* c; a1 t+ M1 m
#define Z_DIR_PIN 12
' F& _  r# M* _& f5 _#define Z_ENABLE_PIN 191 p( r: A- R) M7 R, h1 k  K
' {7 C- Z+ l& i  u
#define U_STEP_PIN 13
, F0 i/ W; k& @2 z/ U, P1 i) V0 O#define U_DIR_PIN 144 M3 t( y& V% A
#define U_ENABLE_PIN 19. U: K0 [4 C4 @. y, L* Z2 j2 u
' f1 [& Q, r% r, r& H/ n
// limits not used right now
: o% K  S  Y& A) v  q#define X_MIN_PIN 14  B# E9 ~; R/ Q. ~, K4 m1 I
#define X_MAX_PIN 14
2 Y5 v9 b4 g5 m5 i5 |( N4 D#define Y_MIN_PIN 140 z& N- L9 J. _: q& H# A8 V
#define Y_MAX_PIN 14: h8 U6 E8 T, S# i  n
#define Z_MIN_PIN 14
: G$ m5 I; X7 J: J- f$ t6 q4 I#define Z_MAX_PIN 143 Z# r) z' l* p9 ]5 I8 d
#define U_MIN_PIN 14
4 Z  N+ t- `% I) f0 \#define U_MAX_PIN 14
! `% ?$ ?8 e* u6 {7 V
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:54:26 | 显示全部楼层
void init_camera()
4 B1 M# e" j8 `* r4 N8 A9 v4 R+ q8 g{
6 X% J7 a+ O% C) w" \  pinMode(CAM_SHUTTER_PIN1, OUTPUT);
$ D  L' }3 s9 q! {, Y8 W  pinMode(CAM_SHUTTER_PIN2, OUTPUT);
7 k. s% P& c" p# ^! K; M  //pinMode(CAM_AUX_PIN1, OUTPUT); // analog 09 L2 g5 c; `& J) f0 w$ d; E3 o
// pinMode(CAM_AUX_PIN2, OUTPUT); // analog 1
: Y/ q% ~5 j' q( z5 Y // pinMode(CAM_AUX_PIN3, OUTPUT); // analog 2. ~8 {5 S8 z1 f
// pinMode(CAM_AUX_PIN4, OUTPUT); // analog 3
0 x3 L- u, c9 f  [% M2 J1 i5 p}
3 t# Z  H+ z. q/ L( x+ b( S) j4 k- y& [& _" w: J
void camera_shutter1()9 D5 g% g. I  e0 `! Z0 J
{
# y' i4 V+ F/ }& \9 k/ x  // fire the camera shutter via relay...1/4 sec hold time
( m) r& `5 y6 a/ V5 a+ N3 g  digitalWrite(CAM_SHUTTER_PIN1, HIGH);
  |  l; f* ]* w) R: D2 {; K4 m- t  delay(250);
4 Q) G" \5 {9 z9 x6 t/ w- F4 S$ `  digitalWrite(CAM_SHUTTER_PIN1, LOW);
5 V4 @% d& a; x
3 q4 U  N6 d1 P  O" i}
  R5 U3 z, \0 n+ S5 v2 @  Q, `" r9 b/ K/ L3 n
void camera_shutter2()
( g9 m* }3 v) j0 y2 M, I& b{
4 ^4 y7 Z3 l6 Y& @  // fire the camera shutter via relay...1/4 sec hold time
3 C  p: y  H* y+ _( [3 r1 O& f! s, ^  digitalWrite(CAM_SHUTTER_PIN2, HIGH);# I: e2 n! ^- Q, W
  delay(250);! E) K) B+ Z( v
  digitalWrite(CAM_SHUTTER_PIN2, LOW);
5 N1 S0 k0 V3 K; c8 N, b# c. {, g7 P- i: i
}
7 \& ~# I. Y& Y* Q, e/*9 V. {8 p/ f3 o: ?: [  M
void camera_aux1_on()4 |. l% a4 r3 ~2 V8 H
{
. ?1 |8 E4 x/ S  // turn aux relay 1 on1 T! [7 h3 U! N
  digitalWrite(CAM_AUX_PIN1, HIGH);
: q* @7 }7 v% ]8 V}2 n; c* ]8 y+ o- X# U3 k- h: R
4 {5 I$ p* W: _
void camera_aux1_off()9 f# {) Z2 c8 G5 e& {
{
7 w2 \9 |4 J, `: {# x  // turn aux relay 1 off% r! |# I0 d; a' {
  digitalWrite(CAM_AUX_PIN1, LOW);
5 m7 b" L3 H% b! S$ @$ S}
3 Z; g9 [& b4 N# f7 B' S
+ j# z! a' y# ovoid camera_aux2_on()3 o; Q& w7 H! \; c. M
{
- j: F6 {6 I7 w1 d  // turn aux relay 2 on
0 ^$ c2 Z% ~) H  digitalWrite(CAM_AUX_PIN2, HIGH);% `2 Q! t# G8 Y  K9 y8 U
}
2 Y, l3 B) H! O; N9 k$ {& m( z+ t7 l9 D* |0 f+ h$ o
void camera_aux2_off()
' e& }0 j& l; Y# Q2 `" @, Z{8 }0 _. J1 S8 [: @9 j
  // turn aux relay 2 off
, d, n2 k7 N6 r1 _* t! ?, T  digitalWrite(CAM_AUX_PIN2, LOW);
: x1 T+ X7 C+ R' \  y& R}
, e' K5 o3 |; V# F( E. z0 E
# c: j# G) H3 ]. ]  ~; ?void camera_aux3_on()
# J5 C' p4 m, U0 a- k{! ?' e* b! f6 D7 @. W
  // turn aux relay 3 on
7 X% `& B6 y2 b2 x) |# R1 x5 I; n# y, F, @  digitalWrite(CAM_AUX_PIN3, HIGH);7 c" v4 t- Y, X* p+ T
}' K+ B/ q0 h& t3 s+ l7 m- M

- Q7 l9 f3 P% V- u& s+ Fvoid camera_aux3_off(): W6 d: y0 \4 R1 ^. [* i8 v4 h8 n
{
0 y; t1 C- _4 L2 w  {2 j- ~4 \  // turn aux relay 3 off
: d% i: I" @- ?) s# W  digitalWrite(CAM_AUX_PIN3, LOW);
: }+ Q3 x* b% v: L6 P}
, S* w8 s( n' @7 G- R' E# j9 `
, Q( X/ ^' ?6 }void camera_aux4_on(); n: ~7 n7 z( [7 O% V
{
# r' H" _" \% i5 `  // turn aux relay 4 on
; i0 p( L# o/ g- a" k. f: n2 `  digitalWrite(CAM_AUX_PIN4, HIGH);
* _8 d: X; h* _# V}# {- L0 r; p1 z. N* g

8 B/ p& Q% |# `8 b) P; `void camera_aux4_off()
) ?: `- [0 h" ^6 |0 R4 ]{( W0 I/ \6 \# N% D6 c2 D; _
  // turn aux relay 4 off6 J3 }2 S, |% j; C( C  t
  digitalWrite(CAM_AUX_PIN4, LOW);  R( b3 @$ J5 g* P' u. a: {% `
}
回复 支持 反对

使用道具 举报

发表于 2014-5-10 09:54:28 | 显示全部楼层
楼主推荐的网址不错6 ]2 y9 c# Z3 c  l, M; J' ]4 e0 g
请问楼主是玩什么的,用乐高玩具吗?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2014-5-10 09:55:02 | 显示全部楼层
// our point structure to make things nice.
; D3 ~6 ^7 L# Vstruct LongPoint {. q' d9 {+ W5 [
  long x;% J/ F5 q9 Z) Q7 E2 l# [' A
  long y;
0 E8 S$ l! M* ^  long z;! d2 M& Q% y! R- F  s
  long u;4 P' P: n, E* Z- m) b4 x" v, G! m/ P
};: e+ C# u: h% o6 {

/ b/ d6 D' L' ]/ V  ?& O- @: ustruct FloatPoint {1 p! C( V# C/ ?1 R' E- H" A
  float x;% m+ X  U: g3 J8 Z
  float y;
6 ~5 w- h3 b# w+ ^5 F! A6 Q  float z;
& [# o# m% l. G4 L! R5 u$ _4 h  float u;
/ e- ~* A$ t9 _5 V! b};
5 w9 D* w2 Z" a; _' I+ A( u+ p# [2 D& ^# g' ]% Q! {$ \
FloatPoint current_units;
; y0 {3 f4 z" S# d& R* UFloatPoint target_units;6 ^# g1 D% D& z* [4 n$ W
FloatPoint delta_units;
3 n; S+ o/ U  S% c6 j8 b$ u5 Q2 W+ y- b
FloatPoint current_steps;* x- J' h1 h1 y, r; s3 U
FloatPoint target_steps;
& i; v: o  Y9 d( J5 W' o* @* cFloatPoint delta_steps;
1 _: q* |' w! r: J7 X5 F: @6 j0 @9 ]6 `8 o/ F. b9 `- _
boolean abs_mode = false;   //0 = 增量位置模式; 1 = 绝对位置模式
: P; E! S$ f  O; ]. j4 P
6 j& J8 p$ a: }. V( y% s//default to inches for units
% ~, G6 l, G6 wfloat x_units = X_STEPS_PER_INCH;
2 N* S7 g/ X3 L/ q" xfloat y_units = Y_STEPS_PER_INCH;) d* y6 ~+ @* q; _" V6 s
float z_units = Z_STEPS_PER_INCH;
1 F( L+ E. `3 I- Afloat u_units = U_STEPS_PER_INCH;% G& y: C3 ]$ Y6 q; L
float curve_section = CURVE_SECTION_INCHES;7 y3 X6 j* u% c, T
; f, n( j8 B% k7 h( E; M. Y) D
//our direction vars
8 n- ~/ K6 Z0 }# R8 N+ Nbyte x_direction = 1;" C5 Y$ a( \' H
byte y_direction = 1;
6 K* n7 E* ~; H3 ]; y) y1 ubyte z_direction = 1;
  S& ~4 p7 f4 pbyte u_direction = 1;& q$ {. @9 T" s% A# V" Y+ m

. |# u7 D+ s* @2 d3 J3 C//初始化字符串处理7 K& m! r1 v4 ^5 @7 k9 W& x+ @
void init_process_string()
# h8 [0 d6 g& c/ _. A{
( J. x& Z( R1 q7 Z& _  //init our command1 z8 Z$ q2 j% G2 Y, H! m* m& s+ |' H
  for (byte i=0; i<COMMAND_SIZE; i++)
' g' S' ?2 J6 g( z4 W    word_old[i] = 0;
3 f5 z3 h- r  i! u; ~/ a  serial_count = 0;
+ R1 g! }% N% e4 S) K}
) y, g  z# F1 E: L0 p/ }* a8 {# z/ @7 T7 \$ j8 K3 V/ _
//our feedrate variables.
6 }: ^; ]* B# X2 ffloat feedrate = 0.0;# T. p' ]: `2 K
long feedrate_micros = 0;# Y( L( ~9 A! q0 b

4 z( d( \% }* G//读取并执行命令
5 I4 Y5 W. h( w5 Z, O- `void process_string(char instruction[], int size)
; Q$ T) y& Q* W- ]6 O  j# `, m3 [{6 x+ v( w1 |3 W; o( V
  //the character / means delete block... used for comments and stuff.
6 B' V  L& p* Q: ]  if (instruction[0] == '/')
& `0 [9 e  N( `( t2 P  {
  R5 x. U' P* d4 O# |  a# U% f    // SerialUSB.print("ok");
( E  ^; v! \7 M3 p    // SerialUSB.print(byte(78));! p* k9 g' v; R, p3 k
    return;
; n# D, R1 L) a6 f4 Q. G+ P9 y  }
0 A5 z: P% [5 q/ ~% S5 [  //init baby!# ~% w7 _7 _( G3 S  s1 V
  FloatPoint fp;
) C- p- W6 j7 }  B$ M0 f  fp.x = 0.0;
0 v1 y6 V" I) f% E; ^  fp.y = 0.0;4 w0 g0 V, w  G7 z% f& g! `0 z
  fp.z = 0.0;% X9 }8 ?: ?8 w7 r  W
  fp.u = 0.0;
. M- x7 ^7 c* T4 T7 b6 _7 e  j5 S/ b
  byte code = 0;
4 d% }- t% x2 T  W1 ?& {. K9 N: j  P# ]7 I/ s
  //显示在处理的命令
* f- y0 ^9 b$ [/ y- i: d#if (LCD_TYPE == 1604 )+ h( b3 Y1 i" U  t
  // lcd.setCursor(0, 4);" K9 [7 L- N! Z, Y
  // lcd.print(word_old);
& n2 j" p7 B2 ]( F! A% ]8 z#endif$ O: M* A3 b4 s) U, |2 ]% e
  SerialUSB.println();
8 |( U2 q1 }' a9 {$ ]  SerialUSB.print(instruction);
$ h: O! E' T) z) O  SerialUSB.print("\t");
6 U+ K% L& X# X$ B+ f- H# m
# v) C6 f# c5 K- i* Y9 s9 V' \& P8 @  //what line are we at?8 `( e, N2 W9 {2 A2 [! A1 t
  //        long line = -1;2 g1 k" j; @6 y0 E; o6 w' [
  //        if (has_command('N', instruction, size)); s/ Y1 z, w1 A% ^1 s8 L
  //                line = (long)search_string('N', instruction, size);  c! _- {7 l0 {$ }

- |- o5 g! R4 P* a; _5 _  /*4 S+ C/ S& O5 w5 q
        Serial.print("line: ");- |+ a5 w9 q  ^. k5 {- t6 A
           Serial.println(line);
! _' b$ K& r6 ?- Q+ R           Serial.println(instruction);4 j" N' n( z# A1 ^4 `
   */
& u% e: C+ N; w1 d6 n0 J  //判断是否读取了个 G代码?+ r( n% h  S0 m. [) R
  if (
0 S$ U% |! B+ p    has_command('G', instruction, size) ||5 d0 f8 \/ F- P: r5 \' q& ^
    has_command('X', instruction, size) ||( f( {9 B: T3 ^$ d( N
    has_command('Y', instruction, size) ||
2 N' r: G4 v1 A7 J( y" N2 Z# {    has_command('Z', instruction, size) ||4 s1 \2 [; b2 x+ U( s. s% M2 `: M
    has_command('U', instruction, size)
1 ]' N1 ^1 n6 v# j) e- p. D' a    )5 d7 C& g+ |* t- m5 H. Z& O, `( `2 F
  {
7 ~: ^1 s1 d1 T) [& R- n    //which one?$ S7 [# R  k( o) M) b8 ]
    code = (int)search_string('G', instruction, size);9 x  u) l5 ^8 T% S
    // Get co-ordinates if required by the code type given
" K% Z: _  F( ^7 v9 A    switch (code)  ]. |& p8 z8 y' d% U2 Y+ e
    {
2 T. p# H0 H# w2 p7 v) F. ~4 U1 A# Q    case 0:! Y; g; t7 G3 [3 Y) F
    case 1:) I9 d5 O. ^. e, u4 b! T" p
    case 2:" L! G9 X. i, p/ Y6 {! z4 `
    case 3:
: c7 U( U  u$ ^- V" a2 _4 r% }      if(abs_mode)
# M& m8 a( r' C$ ?5 w" z      {
9 |, g8 e' B! a: C# @. |        //we do it like this to save time. makes curves better.$ [* k. U/ x+ u4 @: a
        //eg. if only x and y are specified, we dont have to waste time looking up z./ `1 @; T/ }5 d0 N9 o) F
        if (has_command('X', instruction, size))
1 [. l" T% W, f2 ^/ ]          fp.x = search_string('X', instruction, size);3 ?2 A3 c* f3 x5 M3 l5 c
        else# |& w; g' k3 m% V( x2 W
          fp.x = current_units.x;
1 g, F" L8 S* ~, ^0 i: E0 A% \
) W) P- f; X7 n  s, A, k" X        if (has_command('Y', instruction, size))4 K: t0 Z! `; n
          fp.y = search_string('Y', instruction, size);
* I* X) D( K- v9 v        else2 _' O& @* c0 A) W+ w& G
          fp.y = current_units.y;
; O* b; M) H8 ?" H+ ~' s" {" Y& b7 n! G0 [8 m$ U
        if (has_command('Z', instruction, size))
2 i% D3 ]% c3 [          fp.z = search_string('Z', instruction, size);
3 A0 _% W% F- C        else0 M, F% ~; o* W6 N1 A) t4 {
          fp.z = current_units.z;
% p8 ?/ E; K0 ^          ! a8 C5 E% b9 O8 H" x. I& z
        if (has_command('U', instruction, size))
, ]1 [  n# j( O+ s          fp.u = search_string('U', instruction, size);
' ^& r9 W; [) z4 v0 ?7 _* Q; K/ @        else- _$ X% d4 C- y6 g8 X
          fp.u = current_units.u;
7 Q: j% w, @( c' ~& }) a      }) S: G8 P' K: `% i) e0 |
      else
" T+ v% F& _3 m- s      {0 p  Q+ a( x( T: `* m. |- V. w9 z- [
        fp.x = search_string('X', instruction, size) + current_units.x;; `! J' K6 ?  Z8 [
        fp.y = search_string('Y', instruction, size) + current_units.y;. P3 v, t5 {' E" L$ F
        fp.z = search_string('Z', instruction, size) + current_units.z;& l( H, l; h" j3 K
        fp.u = search_string('U', instruction, size) + current_units.u;3 u% {* Z& V: _, ^
      }7 U3 M( ?, q1 S" I- {4 K& k' ^1 ?
      break;5 N- i, y+ v3 p. i" z
    }% f+ C& W  d* e3 P. o
    //do something!2 d+ H9 `! S/ I1 J+ G8 ?- ?
    switch (code)+ u) J8 U) i  K1 t: v/ }& B
    {5 [& s; @2 M1 P
      //Rapid Positioning
9 E3 T- H# G5 ~, Z4 C- ^      //Linear Interpolation( }5 h. B2 }7 z& T7 I: k  s
      //these are basically the same thing.6 o  \: ]! x- e) I( m7 |2 R+ ]
    case 0:
7 [% A0 U, H4 k0 o' w8 ~    case 1:" U: e3 m0 W& O8 t% [$ I
      //set our target.
1 e4 n( F/ M; m7 r      set_target(fp.x, fp.y, fp.z, fp.u);3 K  ^5 ^1 H2 _# d) G
      //set_targeta( fp.a);& i+ u' [) j8 o" ~, U$ r
      //do we have a set speed?
0 W- S5 A6 }0 H+ g- R' T      if (has_command('G', instruction, size))' Q% `8 ^. D  m  T# Q
      {; f: O9 }2 v. e9 ^7 g( Q- t
        //adjust if we have a specific feedrate.
& Z* Q, o! k& @: }9 R4 S        if (code == 1)
' _3 A1 E( {- A, W' ~        {
' T! k; C! Y; h: q1 O  @4 X- I          //how fast do we move?, W% A9 M, u; I
          feedrate = search_string('F', instruction, size);4 B$ F/ n, ~0 t
          if (feedrate > 0)! x7 l) l7 i. f* n
            feedrate_micros = calculate_feedrate_delay(feedrate);. y5 o3 ?3 T6 r
          //nope, no feedrate
5 E5 j4 d- c5 X          else
$ `! o8 a9 }1 ?& R. {1 x            feedrate_micros = getMaxSpeed();: [+ a6 F& o4 q4 G8 ^
        }
6 X1 i" X, d# c1 g- ~' C        //use our max for normal moves.
5 J9 o9 y  y" W4 d- U        else$ |7 C) T5 R# I$ e7 n; p% i0 }
          feedrate_micros = getMaxSpeed();& l* _& o3 p  `( T% J1 Q
      }
: z- ?9 L" X) Q9 M      //nope, just coordinates!
1 w( c- |" Q$ O. E' X9 l      else
; q! W% P( J4 s( N& Y: U, X      {+ ]) T7 Z2 q. f# v9 [5 M7 J
        //do we have a feedrate yet?
# c/ _0 Y1 n, k, j. O+ H# U        if (feedrate > 0)5 L( G' @9 }6 }" P
          feedrate_micros = calculate_feedrate_delay(feedrate);
  d( \6 |  J! N' H8 p# {$ D: `" D" e        //nope, no feedrate
" S( }9 k! e) K5 K        else, @- H# u( ^# {: ~
          feedrate_micros = getMaxSpeed();+ q& Q+ W. G. t: k0 f" t
      }% u0 G1 P( N8 \1 q+ p
. R; W/ L& Z2 R0 |  a% q0 V* O5 B
      //finally move.5 `! Y7 ]4 E- X- d5 J
      dda_move(feedrate_micros);" e& Q& K, u' {, {. Y
      if(stop_flag) return;
/ U6 y* u1 D  Z0 w' s, |( x- m( |1 D7 I' O      break;
$ G: @& ]: O2 }! m) |, ^& s* H3 k9 }0 A0 S
      //Clockwise arc
! @! `( ^* h! H- D! p6 u    case 2:
) N% o1 ~# J; i      //Counterclockwise arc' u7 Z/ @/ r, `, l; X( }
    case 3:
* J* k( q, j6 L* l7 y) Y2 s      FloatPoint cent;1 |  o& W+ K* o1 R/ W
      // Centre coordinates are always relative
: m! r! l7 k. s( W1 r      cent.x = search_string('I', instruction, size) + current_units.x;- D2 A8 `8 F3 {. R9 K
      cent.y = search_string('J', instruction, size) + current_units.y;
4 K; w( k- h; {6 A/ Q0 w8 w+ i, P      float angleA, angleB, angle, radius, length, aX, aY, bX, bY;$ d  Y! X$ \+ t, b; }: G8 t
: r+ w* a! }: [+ w6 X
      aX = (current_units.x - cent.x);
- [* K  s& W) b5 o, P      aY = (current_units.y - cent.y);
$ O. h4 V: G- r+ R. I; H0 z      bX = (fp.x - cent.x);
3 B2 ?7 l& {+ [# I" h- V: p      bY = (fp.y - cent.y);* w9 ?/ W/ B, G  ~
& k8 Z6 |% X: y: x2 Q
      if (code == 2) { // Clockwise
# Y- N% w2 A" R, |' ^        angleA = atan2(bY, bX);; B! _  N0 v) d5 Z1 l! B
        angleB = atan2(aY, aX);- M! ^+ Q3 A" T1 [) G. m) P0 H
      }
' a6 g! l1 T5 I2 H* S      else { // Counterclockwise4 `; U. p% \, x3 f: d
        angleA = atan2(aY, aX);! W8 b/ s5 U8 i1 k2 e- Y- V
        angleB = atan2(bY, bX);
2 R* b: ^9 D; A/ I2 t      }
. |& T& @0 X$ V" `6 C5 A( Z      // Make sure angleB is always greater than angleA
% z3 H2 N& K8 V9 ]      // and if not add 2PI so that it is (this also takes
* Z1 Y/ N* Z- [* K0 M; E/ [      // care of the special case of angleA == angleB,- o0 P' p' g9 ^4 e% T
      // ie we want a complete circle)
7 @# R& S6 ~4 I! k1 @/ N  I      if (angleB <= angleA) angleB += 2 * M_PI;
! P1 _7 O/ ?5 v$ Q3 K      angle = angleB - angleA;0 Z# g+ _$ q" {! h$ A% s
( z2 j, d5 ?& ~
      radius = sqrt(aX * aX + aY * aY);
7 N' w. C0 \+ e2 F      length = radius * angle;% w/ e2 @& X# [* @) F& d
      int steps, s, step;
! q* g$ B- T; C( g# _      steps = (int) ceil(length / curve_section);" E( H( J1 G# s7 Y0 {5 V

2 m. e. ~, B( a6 ]) J      FloatPoint newPoint;
+ \5 h. e9 O" |( n" Y) Y* r* X      for (s = 1; s <= steps; s++) {
4 Z5 S9 Y) Y5 P& X* _+ c) g        step = (code == 3) ? s : steps - s; // Work backwards for CW
" q) _0 W# r" Y* ~7 N        newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));: F" d" j/ k, K# h2 T. v( L2 r
        newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));! w$ |! B3 ^/ @  W( g  s' D: h
        set_target(newPoint.x, newPoint.y, fp.z, fp.u);
9 }9 f. ^$ W2 _! \1 a) `
) p5 i( @) a) r$ J9 p( V        // Need to calculate rate for each section of curve
5 \* F5 c+ Y/ B3 }/ Y: O4 |) d        if (feedrate > 0)7 U9 n2 {0 t" d
          feedrate_micros = calculate_feedrate_delay(feedrate);
3 S0 ~# }: ^) Z; A1 N% z  e3 Z        else
1 T: a- @8 f- Y+ x6 ]          feedrate_micros = getMaxSpeed();
1 V; u) q" C6 Y, n8 S7 w
" ~& X# i3 K6 A6 A        // Make step0 [8 z) n! ]( g7 R/ T
        dda_move(feedrate_micros);
: \  \( t+ u, ?( p) Z, ?        if(stop_flag) return;
, T2 T- o% ~% y# W- d      }
2 U. Q" N0 Y3 d0 L2 X' U6 x9 G4 I" U+ b; u- w/ t' X
      break;8 X3 B* V2 `) }: u# c

0 J4 ]9 P. B! U" c0 {: A      //Dwell
7 i% g( m+ B* a" V    case 4:! r2 z1 j2 E' C
      delay((int)search_string('P', instruction, size));
" y8 s1 D* f* R' ]9 \# X7 Z2 H      break;
' O* x, ~" M0 R' l; N  H: D$ W% k$ V+ M1 L
      //Inches for Units
- ?1 j& k3 A& h7 ?! U" j    case 20:% T0 g; F) G! M: ?% ]. P! H
      x_units = X_STEPS_PER_INCH;
2 X7 q- d# N: w3 P4 R      y_units = Y_STEPS_PER_INCH;
5 V+ @& x: d; s" Z- Q! X/ ]      z_units = Z_STEPS_PER_INCH;
+ x/ e6 {/ M0 Z4 X$ t      u_units = U_STEPS_PER_INCH;+ b, f+ |+ }% j5 S+ e
      curve_section = CURVE_SECTION_INCHES;
) o4 i- b3 C) o( y6 _. E+ i      calculate_deltas();
! X1 X- H9 |# V2 Z3 j      break;  O. D, }/ F1 q1 c2 d4 n

7 ?9 K1 m: h" j" {      //mm for Units6 E, r$ Q* O: q
    case 21:. D2 P1 x# o% R2 Q0 y6 i) X6 O
      x_units = X_STEPS_PER_MM;
  D1 b9 V6 C8 S8 e4 P% X% F      y_units = Y_STEPS_PER_MM;
' p/ T4 @0 h1 d% r7 g/ a6 P: Y6 ^      z_units = Z_STEPS_PER_MM;: L$ X1 z2 ]0 n( N; m; j( `
      u_units = U_STEPS_PER_MM;
6 w+ E) a; m& Z1 G      curve_section = CURVE_SECTION_MM;
% {" E) P2 Q# H2 s      calculate_deltas();- O6 c, x) o  l3 y$ g) A3 a
      break;
6 c( n! X- s' R' Z( d+ R+ ?9 w
, l" l* l% @/ Y" b      //go home.
. T$ |; L3 |9 {/ q1 C5 F    case 28:
& f$ Z7 ]) `- z+ p- \      set_target(0.0, 0.0, 0.0, 0.0);
9 Z' \' w5 e% g# A# x      dda_move(getMaxSpeed());4 N4 V# _* ]/ X; s' I' n) ]
      if(stop_flag) return;
6 I& C: N! O* L0 S$ ]) L  s      break;
9 i; Q, p! x' V7 l4 ?! P1 ?6 D! u3 Q+ j
      //go home via an intermediate point.
9 n. R" z( _" f$ |% _9 ?0 W    case 30:
6 |6 C3 ?: O* P: t% g  F* F      fp.x = search_string('X', instruction, size);
! c9 e* z9 q* |/ V/ y& Y      fp.y = search_string('Y', instruction, size);! L" c# j; Y! b" q" s" ^4 S& }% W
      fp.z = search_string('Z', instruction, size);
0 Q; R- N+ w3 P" @% @      fp.u = search_string('U', instruction, size);4 {5 U2 U  l1 b) ?. V1 \$ t3 N
      //set our target.; h7 J% B! u  S# G
      if(abs_mode): T! k( p" k1 |" d8 C9 {
      {
8 B- V, Y% P; q8 v5 I( f! @        if (!has_command('X', instruction, size))2 G4 K8 R6 X$ u% X- o: L7 |
          fp.x = current_units.x;
! d: o, V; D3 f9 {4 y        if (!has_command('Y', instruction, size))
, \& [# V' N$ i- u5 j8 E          fp.y = current_units.y;
& w5 m. V( \' q  N% V. W: B6 y        if (!has_command('Z', instruction, size))
; i1 p. I8 Y- [8 `' a) N2 D) D          fp.z = current_units.z;! }, N% _% c% D% M7 G" o& T$ o6 k4 T
        if (!has_command('U', instruction, size))
1 U4 `  |. j  ^1 N, \: h          fp.u = current_units.u;+ C: u0 I  Y8 L- D1 r* O
        set_target(fp.x, fp.y, fp.z, fp.u);7 y+ ~3 E0 Z* p$ `/ T5 }9 C
        * l. \; o, T2 q9 [
      }
9 f. ~! ]( ^( _, y: C; J* X      else
8 Q9 S* v( v& y  H7 \        set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );4 G' p( u0 T' `" {
      
% p. E* _) x7 D. T, i6 I      //go there.* Z1 d5 ?/ v( A
      dda_move(getMaxSpeed());
0 A. _! p1 v) l7 [* E) n      if(stop_flag) return;
) O7 e+ u! [& W$ v  @1 x7 M
6 o& w9 j. N# ]7 E8 E      //go home.
1 P3 c* h) D2 \" D      set_target(0.0, 0.0, 0.0, 0.0 );
% o' _/ c2 s( a; J( P     # N7 b0 U! Z% K
      dda_move(getMaxSpeed());
, I. B+ c- j( r7 E3 `      if(stop_flag) return;
3 u# j6 {! B2 O7 f      break;# I! f) q" ]& X3 f, k# m

$ ^8 @6 |2 a$ a5 h9 ^      //Absolute Positioning
+ y7 Q' f1 [# k, _9 Z4 a0 \    case 90:
+ F6 B1 w: \4 T; d: y) l& B( U7 S      abs_mode = true;% C! \7 w% R/ U
      break;4 f8 X0 ?7 A) b1 e! \. E0 C
% _: @7 p: g+ y( O4 o" U
      //Incremental Positioning
# I- U/ I6 G2 c- \' z4 ]4 V5 k    case 91:& D; k0 r1 Q3 @
      abs_mode = false;4 f+ I( h# d8 M3 }& B
      break;: F8 \8 ]# h: s! r9 ^1 {9 i2 {

: \* d' R2 m' g* C+ [! |      //Set as home
+ Z5 m# X! c5 P    case 92:$ G8 }( q* g; M
     5 f0 b, b! f- E7 y0 ?4 J
      set_position(0.0, 0.0, 0.0, 0.0 );
. A  ]) Q5 V9 t( x      
- c! d% Q6 L9 p  V0 P: y: ?      break;
" Q6 V& `1 f" m! }6 s. s0 T
- \, j2 x6 w0 j      /*" ~2 J4 E5 T: B; N  I7 b+ \) N
                        //Inverse Time Feed Mode9 B- L; `# Z" P, p# c! ^
                               case 93:
3 F3 C( H: v( Z* v9 X       8 A( H3 n- r! }8 C5 t4 H% \
                               break;  //TODO: add this
, n+ o! N8 B- v; M* I         Z( j* k: G8 ?& K9 Y- [" ~
                               //Feed per Minute Mode5 Z/ d9 p2 \( v: q& Z9 n
                               case 94:
% r. v9 |- i, U$ P/ l5 M7 i  z+ K# R      
' O( S0 a0 r" x2 E' L                               break;  //TODO: add this# f+ Y  [7 E2 s5 ?& r
       */
  F  @! ~' T3 q; S
+ U9 T/ @- ^, I5 v7 A7 i1 D6 h    default:# @" Z( g# t! p
      SerialUSB.print("huh? G");
1 M4 \) Y8 q. p, a+ V      SerialUSB.println(code,DEC);
6 n% {2 I5 V! f: D9 J. g5 }  P    }6 T% s3 H( M$ X: Y3 Y2 Y
  }
# q. T: ^( r) j% }; ]: j1 a/ g
: q% a" _! e* `0 O  //find us an m code.
% ^: z' _+ l4 n7 X# L# e3 p  if (has_command('M', instruction, size))
* s6 E- W& G9 P+ E  {" e$ k( ^% C: V: k- K" c& A) X
    code = search_string('M', instruction, size);! v3 S6 ^5 K+ h: e) z
    switch (code)
2 o: a7 `' }7 Z( R2 G5 @) L) o, j9 V: B    {
; \! \9 _: Y5 r1 G      //TODO: this is a bug because search_string returns 0.  gotta fix that.* M2 s& D% |" W. [5 m
    case 0:
9 z8 T* N. C; j9 k. W      true;9 o/ M. ?7 x5 ^; {; I% N2 H3 k
      break;1 c! _) X) w: n' E8 o

- O9 t! H- I- I) R+ |( e    case 100:
0 Q* z- Z4 V& t: _/ ^      break;4 I8 c' {4 r$ X5 A4 r) e
0 a+ A+ F# j; U" l' ]0 G
      // fire camera relay
7 S) b! W+ {  U, y* A& X    case 101:6 p& `3 T7 p. R
      camera_shutter1();. g3 ?. K) X) P  s
      break;$ g* ]7 a: Z/ Q& A
2 z# q# V/ g. H4 T7 e$ a
      // fire camera relay2
/ Y" `% B8 w. D( x7 a* j. @    case 102:
0 {: u' ]) `/ c  I! J1 \  k6 k% K      camera_shutter2();
8 |) t$ z$ b( S. j# `; d$ D; j3 B      break;
0 p* f! i: u: @5 f7 h! Z/*7 ?5 |& G  k' @6 p4 Q7 K: Y4 W
      // turn aux 1 relay on
! {' W5 ?! T4 `' Z    case 103:
. [9 H3 [3 e# J: ]7 _4 R: M3 w      camera_aux1_on();
, s0 K% }$ N5 h6 z* S      break;
$ Z+ b  B0 ]1 o" ?# ~6 L
  E0 N" I# ?! I' p# v      // turn aux 1 relay off
+ J1 h' y# a# }5 V9 _1 ]3 y0 B    case 104:6 }7 E3 P+ g, I5 a8 N$ U" H
      camera_aux1_off();
; v0 B. \8 P* p% E      break;- ~7 h; e: y+ f9 A% Y& R3 M: H2 x* u

; T: P3 H/ o) g5 [0 h8 S      // turn aux 2 relay on# a+ d0 R; ]- H/ S9 i. w
    case 105:: A. [) D- u1 A7 r3 c
      camera_aux2_on();0 F) t+ r; h  e9 p& f0 ^8 d8 O
      break;7 W+ j7 J+ T  s0 E" {. r2 |3 y
7 J5 W# w" z" R: y
      // turn aux 2 relay off
% A: _9 i0 L+ w) q  p7 ~    case 106:' p  ^. }/ a% L1 f/ t
      camera_aux2_off();
. K) b1 d: {4 y0 m. X3 c5 v      break;
9 n) l- h0 U& f, B# n3 z; I
2 @/ J. B4 d- S8 A3 D( @      // turn aux 3 relay on
) Y7 Z2 u8 u* X! n8 L    case 107:7 X. Y9 s* `, U' \- R# O
      camera_aux3_on();
: a" x/ [- Z, o5 T: s! E. ^      break;/ f* O$ `3 _4 `* i* B6 j+ x1 R

( x- H3 o, t/ B4 n- {+ ~2 g; H      // turn aux 3 relay off
6 D9 u" [1 j0 d9 T& q    case 108:
3 r! r3 W" G4 M' E9 J, `      camera_aux3_off();
# O: J0 W1 H* r1 {- D" [      break;
. L0 m* I/ v- \" ~4 r, O  x' F/ y! U* P, L: b
      // turn aux 4 relay on4 S$ `6 o4 }% G5 C. r( d
    case 109:5 b3 W, |1 c& d" x& U9 c2 I
      camera_aux4_on();7 X, l( j. W2 R
      break;
. l# u5 `3 _2 m" r
1 S! g$ g: O) Z& _: _/ j, b      // turn aux 4 relay off
- w. [6 j  l+ O  G2 ^  D" Y    case 110:8 O/ a. k7 {! R4 G
      camera_aux4_off();, E" y- j7 ~- A" c
      break;
. }  a' K! F5 @# r  \+ K*/
( I, `- H3 w7 N. i: r8 M* G    default:; _' Z0 Z, ^6 C8 n2 f
  ]+ ]9 K  y5 d, i
      SerialUSB.print("Huh? M");
0 }) }0 [# o, ]3 n" t      SerialUSB.println(code);+ I; I) i: B9 K* T2 q
    }/ C3 ~! K$ C  e, C
  }
: @2 G6 Q+ `# N& Y9 W# p* K! v
  //tell our host we're done.7 n5 Z) Q  h. G
  SerialUSB.print(byte(78));
5 L: P( l" z3 Q2 J2 }/ X0 ]. M' R8 B: s0 b& f
}
6 d; Y2 Q. s+ d+ A. d) B  U  k5 s& R1 `. }! H8 G( B( O
//look for the number that appears after the char key and return it4 N8 Q0 m. x- j' m6 y% Y5 g8 ?
double search_string(char key, char instruction[], int string_size)
4 ~& s- l8 F' m; A+ w0 Z7 _% }{9 f% g* s4 Y/ B0 A
  char temp[10] = "         ";
6 x1 S) `* T/ n; B- }% ^  for (byte i=0; i<string_size; i++)# J5 t' f$ D4 [" u
  {$ d/ v+ W- T! N8 W. |+ W5 \$ v
    if (instruction[i] == key)
# k6 N5 {* A- B    {" |+ c" x3 |' R. R, a8 h
      i++;      7 x5 U+ J) x! g+ T" m
      int k = 0;' ?& Q2 V9 ]! S) o2 q4 I/ z
      while (i < string_size && k < 10)  K0 w) L1 X; V! _# W3 ?% M* m
      {
  a) C% U( w* ^% `        if (instruction[i] == 0 || instruction[i] == ' ')8 C6 ?  Q! M. N, r+ N
          break;
6 p  |( W5 g% j. K$ {
, [6 P4 c3 h. a4 m2 V. S) I        temp[k] = instruction[i];0 Q, v9 I5 J3 j. w' D0 k
        i++;/ G* a; K( B( _' @5 s  V
        k++;
9 Q; N! e+ r. A, A      }0 j( y2 @4 O$ [/ e- J, [" i; @( J
      return strtod(temp, NULL);0 l% U: V+ ]5 Q
    }4 \4 V" w5 d5 `. n  b2 P
  }; S5 w) Z( F/ z, a  q) N, x
. V2 N7 D# v; c
  return 0;' }- h7 k8 S4 e
}
( p& a$ W# [# F8 [; [, S$ g0 I: Q& C, Y& ]9 E
//look for the command if it exists.
, _( `' b3 k: y6 e" Ubool has_command(char key, char instruction[], int string_size)
) n( s- ]$ x" B7 o) G0 j{
! A5 R) S1 J# B4 X( t+ M  for (byte i=0; i<string_size; i++)
/ v- `9 T' \+ H8 i) u7 w  {
  n# G8 G. x9 c( ~7 A4 p. e    if (instruction[i] == key){
( I. P7 m1 t( V" @
9 K) V  \2 Y6 F8 C5 p; M      return true;
7 x% i# e/ o* G    }% e+ N, s/ H" i; a. B( t0 X9 V# o
  }2 c; m" R7 ~. \. a$ O

. U+ A/ a8 z  Y& L4 n  return false;' `- X; Q" M2 s4 r. X
}
- }$ [0 T; W! R6 X3 u5 q
& n! c- H+ l+ w! u7 `1 s8 b4 G6 }2 I, i8 N  V
( ~% t0 z& u5 T6 x) w! n! O
回复 支持 反对

使用道具 举报

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

% j: R( c: A9 h! q; Y. E$ b9 F//init our variables' ~; ?, b( W/ b
long max_delta;. h: p9 D+ P: L: x# d
long x_counter;
+ \( a, A3 m$ ~1 F0 hlong y_counter;5 N3 A: f$ r  H4 m: m1 b, M, V
long z_counter;
0 ~% y" |# H8 e  E8 ]! Ulong u_counter;5 t: h$ Z& J9 O* V; {# F
long x_pos = 0; // x position in terms of absoloute motor stepps7 _; g- k* i) b: H% `/ w4 W4 I
long y_pos = 0; // y position in terms of absoloute motor stepps
/ a: j& N, y; vlong z_pos = 0; // z position in terms of absoloute motor stepps, s2 E: F( O2 k% |" y1 @
long u_pos = 0; // U position in terms of absoloute motor stepps
( b) |8 n; w, Y1 [$ T; t; H: N/ Z. J4 N# ]
bool x_can_step;
& _2 [5 P) K( y6 ebool y_can_step;# M7 f; j4 T9 z1 K
bool z_can_step;
# H6 f5 ~$ @7 m0 {1 ybool u_can_step;: N9 z& O& Z+ y( C1 b
int milli_delay;0 `; l/ W8 f( O
! u+ C) U/ h1 F0 m' B+ v- Y
void init_steppers()3 q6 {7 T" M; I+ r( T. L
{% q/ n8 g5 I. R  J# |. e+ _1 o! \$ p
  //turn them off to start.& c0 z/ m. X3 C5 _1 p
  disable_steppers();# |$ p$ C0 ^7 D# Z3 ]; w- a

0 i/ ]" c1 N2 v$ M% H3 I  //init our points.; G: ~/ A8 n- |' T$ D# U
  current_units.x = 0.0;. b) f( V1 |9 ?1 d, W) c% G) Y
  current_units.y = 0.0;
2 g' \+ N& J; L+ N/ ?  current_units.z = 0.0;7 |) z7 d, D" N; F7 d2 {
  current_units.u = 0.0;
8 `8 |& K% v+ F# A9 M( M, Y  target_units.x = 0.0;. j( t9 k7 E1 L& N. o, W
  target_units.y = 0.0;4 A& t+ b9 d+ R+ @3 Y0 F* g
  target_units.z = 0.0;* e' o: }( U3 Q& b
  target_units.u = 0.0;
) c" I0 m8 N! E5 b: W" J. d  
; }3 k! @8 K5 b
2 O! ]. @+ G+ w2 p1 H- D/ M4 i  pinMode(X_STEP_PIN, OUTPUT);  O/ }: C! i' M! E: \1 C/ ~+ I
  pinMode(X_DIR_PIN, OUTPUT);. V9 E; Y! }1 y
  pinMode(X_ENABLE_PIN, OUTPUT);
5 S' r4 w" h" ~  M7 X3 }* [  pinMode(X_MIN_PIN, INPUT);. }$ v* }9 ]# o, R- s/ c
  pinMode(X_MAX_PIN, INPUT);( N; s8 S) \2 N' |2 J! c

3 d9 o7 Q# c. r- G! R1 R  pinMode(Y_STEP_PIN, OUTPUT);; y# C: W+ F* |( G# N
  pinMode(Y_DIR_PIN, OUTPUT);8 w* r4 \% Z/ D! s* n. b
  pinMode(Y_ENABLE_PIN, OUTPUT);
2 }6 V9 i1 e* ~- [1 A0 T  pinMode(Y_MIN_PIN, INPUT);5 V3 N9 P) U# S3 s% X; M& p- k
  pinMode(Y_MAX_PIN, INPUT);
) T2 N; c# s  x: u8 I- N
% F/ I  y/ u  {% L6 A+ i0 r  pinMode(Z_STEP_PIN, OUTPUT);
  }1 y; K- v! f: z! t/ v  pinMode(Z_DIR_PIN, OUTPUT);, G, L* @+ I& x( O* B
  pinMode(Z_ENABLE_PIN, OUTPUT);
$ W7 m# K9 g$ _, k; {  pinMode(Z_MIN_PIN, INPUT);
$ P. ^2 B( d  }' `7 P  pinMode(Z_MAX_PIN, INPUT);
4 X0 w1 P, U, d- i( F6 Q  l" b
5 V" H3 f" {( t) u. H  pinMode(U_STEP_PIN, OUTPUT);& r- r, F) A& H- o
  pinMode(U_DIR_PIN, OUTPUT);; E# ?% B& j& D$ N; }1 d' W
  pinMode(U_ENABLE_PIN, OUTPUT);
: }( U  \# F! d0 \; R" \# X# G  pinMode(U_MIN_PIN, INPUT);
/ [/ H9 t3 r4 L# i2 L  pinMode(U_MAX_PIN, INPUT);( T$ W7 t  S" V* O# T0 b
  //figure our stuff.8 N) W& }* G6 h0 u
  calculate_deltas();% F' i6 \8 }1 E4 C
}3 }: ^: A2 Z+ Z- P5 P3 t
: c, Z- T2 O" m: K  R5 Q% e4 Y  b
void dda_move(long micro_delay)
* g; f0 H. r& Q$ J: V$ l: E" H0 u' e{$ Q5 `* ]- u9 w
  //enable our steppers( f; h! Z# d: q7 }* {0 x
  digitalWrite(X_ENABLE_PIN, HIGH);
7 `- G7 Y( U2 H; P  digitalWrite(Y_ENABLE_PIN, HIGH);0 W% b4 g5 |8 a& C9 M% r
  digitalWrite(Z_ENABLE_PIN, HIGH);8 M. C4 G3 W. j# j  m0 Q. K7 D
  digitalWrite(U_ENABLE_PIN, HIGH);
: k. X& _- i' A! q% ?  p5 D& \  //figure out our deltas
' }* ?* c0 p: n/ y$ w# ]" T  max_delta = max(delta_steps.x, delta_steps.y);
' y! l* X# D/ v  max_delta = max(delta_steps.z, max_delta);7 \: w) ^  s: L4 ?- d
  max_delta = max(delta_steps.u, max_delta);& S5 Y- `1 l2 z# e
  //init stuff./ C1 O# s+ C! X3 h! v( P0 @2 U; @
  long x_counter = -max_delta/2;0 M) u8 l5 N* x0 l9 |; |
  long y_counter = -max_delta/2;
+ O& V! E- f7 T8 r  long z_counter = -max_delta/2;
) S; k$ V2 i; x" y  long u_counter = -max_delta/2;' O! a* C; i- Z$ r: ?

2 F. c, x/ c7 [. Q9 _8 a  //our step flags
: _; D, |0 L" D6 {  r$ K  bool x_can_step = 0;; u0 ~/ b% g/ O! O, C! c
  bool y_can_step = 0;) J: m& ?/ k, K- I$ e
  bool z_can_step = 0;
% A2 d: N3 f  j  bool u_can_step = 0;& t7 ^1 a) P0 z$ T

& i+ @5 C5 ~. w/ {  if (micro_delay >= 16383)5 m* `) C0 _& ]& Y: K' i+ w
    milli_delay = micro_delay / 1000;
8 \+ p; T' P' w- v0 y  J( o  else
* p, r* L2 z7 x$ e, J  x! u1 D    milli_delay = 0;
! K+ f; N/ F( Y- l( U8 w# i5 |' s% P

+ T* C0 R6 V4 C& [- n  //do our DDA line!3 L  I% ^. p) i, V- x$ H3 N! _
' K! j0 P0 |% n  J& Z. @. N
  do
3 Z6 |# N4 |& \; _  {
& x3 G" @4 N- D/ H    if(( digitalRead(BUTTON_SS)?analogRead(BUTTON_MAN)>>9==0:0) || stop_flag) break;; j. ]5 p6 h6 g+ D
    x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);( I! d. s9 S# ?/ m2 W, B
    y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
1 w2 I; p# o! b: u: S    z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
( |  `" M0 E5 c& \/ h    u_can_step = can_step(U_MIN_PIN, U_MAX_PIN, current_steps.u, target_steps.u, u_direction);
  F8 W( e, u$ p& Q% o# X1 P    if (x_can_step)
. r- F! e. M) g    {
; {' X2 T5 G! S5 @' K2 }      x_counter += delta_steps.x;. S! i/ I: V$ {: Q0 u0 R

3 ]1 ]7 n7 A9 g& }      if (x_counter > 0): E- X+ G% @. v# f5 [* N  t' g+ v6 X
      {& c1 z) H5 N# }  @
        do_step(X_STEP_PIN);
8 ?# E) K, a3 c        x_counter -= max_delta;
! |8 n' N& P# f7 u% L5 k' c8 X        if (x_direction)
% G- j0 v( J9 B7 `: w% B         { current_steps.x++; x_pos++; }8 r6 ?, G9 \' w% y
         
8 t$ y% U$ A; d# T6 l1 C+ H        else
" V$ |8 c6 h6 ?- V          { current_steps.x--; x_pos--; }
& N0 a! `" d  R         
! `- Y2 e: n2 n' d7 B      }9 X* _5 {! G, a) b
    }+ q( D5 z2 N' U1 @$ @: M! s3 m
    if (y_can_step)
' y+ z5 q+ f9 J9 L- D- f, H$ e    {
  C: O) c  \" @. {8 k      y_counter += delta_steps.y;
0 ~" |- L( O" F9 Y. S+ B# ~" s5 o
3 h  e7 D4 c4 l; m/ R      if (y_counter > 0)
5 M# z9 ^7 {6 a      {$ X1 Q6 L2 ^0 n9 {
        do_step(Y_STEP_PIN);, i- N9 Y+ c- {% E  k& ^- H
        y_counter -= max_delta;2 Y9 i% _6 g3 F
% w" b/ S/ Q, |6 M5 y: A1 ?# r$ H: v
        if (y_direction). m0 X8 B% `& G& F8 ]
        { current_steps.y++; y_pos++; }
0 u/ n- I. G9 q* ?* Y; _       ; u+ l  A$ r, n+ _" Q) ]9 }) G9 {7 k
        else
' h0 t% n' G; B8 ^8 C" _        { current_steps.y--; y_pos--; }
6 d! E! y$ i' n3 F8 K- q          d) q4 @1 [& W# y; Z
      }
% Z: H6 y) o4 {/ R( _2 ]    }
/ I/ s$ Z8 r+ n8 t+ x
1 `' o4 W; z% O    if (z_can_step)  n$ c$ x6 s& {7 l, p
    {5 H, ~$ p  M( |% r& n7 j
      z_counter += delta_steps.z;
' s8 y2 |+ a- V& ^- e2 Y; {+ P2 f
      if (z_counter > 0)
6 P! s6 d- U# O3 V# x      {+ e6 w2 G5 |9 o
        do_step(Z_STEP_PIN);
# r4 j5 _) C! [  i        z_counter -= max_delta;
" `2 g" ^2 @6 m* ^1 L- Z  Q2 b4 R
. L3 l4 B' O, \; ^; r& D6 H        if (z_direction)
# h" E4 Z$ ]% i) d! E3 E) ?        { current_steps.z++; z_pos++; }
' y) R+ u  ?3 e6 ^  ^' X        3 k5 `( G# k2 H& C7 T1 ~, P+ q
        else9 \1 ]6 \; s4 ^' g/ a$ T9 w
        { current_steps.z--; z_pos--; }( p/ D2 E2 T* m6 P9 A! o
        
# x8 s$ K9 |6 A% c      }/ h8 K& `" P$ y; A  G
    }2 n; f2 _7 a  z
    2 j! u$ r  G+ x' r7 v6 X
    if (u_can_step)5 I$ W/ R  d/ h% ]9 N) c
    {6 O, ^/ x9 x+ u; x
      u_counter += delta_steps.u;
. a6 ^4 l( `9 k( @* m+ j5 l# I/ ~' p
      if (u_counter > 0)
0 A# {- N+ L/ r5 E2 M3 V      {! t1 W5 h' Y, J
        do_step(U_STEP_PIN);
$ p+ [. B4 |' L% N& c5 y7 @4 B        u_counter -= max_delta;
5 D! i( L6 D) Q5 u- c' F. O9 R  w/ J1 s
        if (u_direction)) `" S$ z% b; \6 B! O. {
        { current_steps.u++; u_pos++; }
# D1 p$ _: G# w         
' U1 m: w8 R) H) l5 B2 Y  ^3 r3 e. Q% I        else
3 {9 m2 O; }3 y! u" l% ~2 F' B          { current_steps.u--; u_pos--; }
8 q$ z; ?% a2 B% J2 o         ! w3 {/ `( F- r% A, y/ s
      }3 Y; w$ |4 P* l/ K+ D% e7 o* Q
    }" B, o5 u* ?" m4 ^5 [1 w
    //wait for next step.
+ G) r+ L' M5 H6 }    if (milli_delay > 0){
+ q  p% x* C: ^$ E6 s! o6 c$ C      //if (digitalRead(BUTTON_SP_EN)) SPEEN();' m7 U; W  P% w0 x. c, X  p, Q
      delay(milli_delay);
; i! K4 _- }. {% Y+ V8 Q    }                & t1 y$ _& k0 r4 R  e2 V; Z! _5 o0 X8 @
    else{# C' O. L! ~+ t
      //if (digitalRead(BUTTON_SP_EN)) SPEEN();
+ v$ x! M: e1 H) X* B6 T: _      if(micro_delay>0)delayMicroseconds(micro_delay);
" L4 c# a0 a! {' I# _/ D    }
, B" Y* a  _7 g. w6 @4 O    //if(x_can_step%40 || y_can_step%40 || z_can_step%40);* P5 A9 A7 J: G0 j: |# ?
  }! \$ i  M7 [6 b* i# R6 Y
  while (x_can_step || y_can_step || z_can_step || u_can_step);- S1 }2 V5 X) u! \

* Y- X1 @3 e) g$ N; ]  n7 D1 M! X: J- r6 Q; D6 W3 c  J
  //set our points to be the same
, e* E: t. I! q4 g8 Y  current_units.x = (float) x_pos / X_STEPS_PER_INCH;
' x, B& @$ ]" v# j  current_units.y = (float) y_pos / Y_STEPS_PER_INCH;
/ V6 ~" p0 c- w( f( K6 |5 ?4 J  current_units.z = (float) z_pos / Z_STEPS_PER_INCH;
4 Q9 w* ~3 B! k7 l4 T! e  current_units.u = (float) u_pos / U_STEPS_PER_INCH;
& c/ @5 G4 E2 X0 f& c0 a  
. {# Y  M! y0 T! J* a  set_position(current_units.x, current_units.y, current_units.z, current_units.u );
  p! D% b6 r% m  f( g7 I+ o/ g + R( g, m/ A, u: D8 ?/ K* F
  long x_pos = 0; // x position in terms of absoloute motor stepps' o$ n" I. l7 t9 o
  long y_pos = 0; // y position in terms of absoloute motor stepps. i6 A/ \: h' P$ I1 a
  long z_pos = 0; // z position in terms of absoloute motor stepps
' c: p& k) S/ h; y  H  long u_pos = 0; // u position in terms of absoloute motor stepps  f7 A! Z( q- l7 D+ x# S
  calculate_deltas();
0 O6 b! S6 o. k1 y  l9 Y- m. E( p  
/ w4 U0 j0 Z7 p}
' l! {+ a# a2 D& t6 h
5 g. S! f' E9 N" x% v0 i) E2 k! }bool can_step(byte min_pin, byte max_pin, long current, long target, byte direction): E8 c* b" K7 {/ d0 P  ^( G
{
1 X& }' t" U/ a  f  i! L7 x( r2 N  //stop us if we're on target
! C& \0 M1 O' O* T; B  if (target == current)3 g! E. K, {. ^( r# v" g& l) o0 S
    return false;2 W2 z5 l0 E" b
  //stop us if we're at home and still going   D# R) z6 ?! ^6 q0 f$ I
  else if (read_switch(min_pin) && !direction)! f% _8 M8 o  m" P  z
    return false;5 j* a7 ]  i3 G7 n5 W
  //stop us if we're at max and still going
% @: O9 Z% D" d! u9 j  else if (read_switch(max_pin) && direction)3 G; G$ d$ w( Z. E
    return false;8 \0 s5 `$ o& V5 s

: \$ I7 c) _( u( v: O: F  //default to being able to step9 |: R+ v. w& y9 P% x# ?
  return true;* _& J6 ]" o2 @2 R
}6 l6 A! D: q' e- V" k
' s8 i9 ?, v1 U9 l3 K
void do_step(byte step_pin)$ e  Q8 n& k: ?% Q/ w/ V. C
{" p5 y' g3 D1 c& F
  digitalWrite(step_pin, HIGH);2 Z5 D& h% U0 q* y  R" v( b  K! O
  //delayMicroseconds(1);/ M/ B& o  X+ c* K* b
  digitalWrite(step_pin, LOW);  p5 y8 M( }9 C/ |; ?9 Q
}
( U4 M7 w& f+ y  ]2 G1 Z2 |# z7 C8 ]) j. k9 S
bool read_switch(byte pin)0 E' f1 W& a2 F( g3 h- {3 B) f/ |0 g
{4 j  f4 Q/ `/ ^- x
  //dual read as crude debounce
' H' `, K( `; n$ H' }: Q8 G7 U- b: c1 x' n5 }4 v% d
  if ( SENSORS_INVERTING )
4 W5 D3 f' C* j: L  l2 @: |    return !digitalRead(pin) && !digitalRead(pin);6 `3 }& O0 N: k) v; t. D2 X7 A
  else
6 X: G; ~+ E9 M    return digitalRead(pin) && digitalRead(pin);* y3 O# a* d' Q" D4 E, H% `0 v3 O
}% i) M7 t- [/ h8 @* j! y1 i

; \; F) X1 G: ~5 g: t2 Jlong to_steps(float steps_per_unit, float units)* c6 H5 I# X* r
{) P$ S% C; E. R( s! p3 b8 r
  return steps_per_unit * units;. w; \$ M/ B- n: O$ F; }7 r. Y: _
}+ j: j! x) J" l# D0 c) P

: R" z1 e' e5 c* Uvoid set_target(float x, float y, float z, float u)
% e( \: G5 _, `+ w7 `6 W# X$ ]( T5 B{2 d2 G& H8 }, T3 L! o5 P
  target_units.x = x;
* `$ R  o7 m$ p' C- W! m  target_units.y = y;
* ^/ @( d$ y- h1 R; W5 I' u  target_units.z = z;
  t4 F& |$ ?3 h; P  target_units.u = u;- w/ S* Z. _- p4 g' }; W3 Z1 r
  calculate_deltas();  |! x# v1 f8 v/ l1 {; B  I8 \
}
% c8 S4 n9 f0 h, T3 s0 s
( Q/ I" {/ V3 v: Qvoid set_position(float x, float y, float z, float u)
! M& A& |5 f4 b# a% ^) K5 \{: ~, X+ {4 t* B9 ?  s6 I7 k
  current_units.x = x;
7 J% k" G: ~2 w  current_units.y = y;& u, |: J, h- p' Z( C1 K; B0 f
  current_units.z = z;- M) }' y8 U0 ^( V; J5 @# H
  current_units.u = u;1 W/ k" o3 [3 P( y' l( {8 ~8 Z
  calculate_deltas();. ~  v4 A. l( a- Q* W8 B
}
" x1 L) t3 j5 u3 i6 T( ]7 ]% f* X; k/ h& b
void calculate_deltas()0 j5 t3 }9 \0 P
{
/ n) `# `+ s7 a8 |  z  //figure our deltas.7 T+ R0 I9 ]# Z- `7 R' w
  delta_units.x = (target_units.x >= current_units.x) ? (target_units.x - current_units.x) : (current_units.x - target_units.x);
5 N, h  h2 ]; T) b0 W  delta_units.y = (target_units.y >= current_units.y) ? (target_units.y - current_units.y) : (current_units.y - target_units.y);
, J- M/ Z+ g( V  delta_units.z = (target_units.z >= current_units.z) ? (target_units.z - current_units.z) : (current_units.z - target_units.z);
  R* Z* J/ u6 ~( Q1 m; |$ i- M  delta_units.u = (target_units.u >= current_units.u) ? (target_units.u - current_units.u) : (current_units.u - target_units.u);
8 L, F9 t+ E% }9 J& E1 o( w) [' l
( i2 j3 @& A$ [  //set our steps current, target, and delta6 T- U4 S- ^" r: j/ X8 u+ b
  current_steps.x = to_steps(x_units, current_units.x);
3 B% O" m4 g* I: e8 _& x& l  current_steps.y = to_steps(y_units, current_units.y);
1 C. N( t+ e3 a% A( {: a; W  current_steps.z = to_steps(z_units, current_units.z);4 T( U3 N! C2 e* X7 k
  current_steps.u = to_steps(u_units, current_units.u);) G$ E/ z: ~$ X5 g: ]9 P

% L( i1 C, Q8 @* v: s- \2 }% n  target_steps.x = to_steps(x_units, target_units.x);
8 Z3 j9 t' p. W$ j  target_steps.y = to_steps(y_units, target_units.y);
5 a! Z* W9 z! \& G0 ?  target_steps.z = to_steps(z_units, target_units.z);
9 L: E7 n0 a; @4 F8 F: P$ V  target_steps.u = to_steps(u_units, target_units.u);+ P' z3 x9 Z6 |5 D
2 P& {2 L2 P) f5 d7 q( U! S. g2 @
  delta_steps.x = (target_steps.x >= current_steps.x) ? (target_steps.x - current_steps.x) : (current_steps.x - target_steps.x);5 K1 W' N' ~7 x. X7 v
  delta_steps.y = (target_steps.y >= current_steps.y) ? (target_steps.y - current_steps.y) : (current_steps.y - target_steps.y);
. {: }2 Y( L  g' b% _* y  z  delta_steps.z = (target_steps.z >= current_steps.z) ? (target_steps.z - current_steps.z) : (current_steps.z - target_steps.z);
" J7 Y" [- F! e0 C8 D6 |' ~7 j  delta_steps.u = (target_steps.u >= current_steps.u) ? (target_steps.u - current_steps.u) : (current_steps.u - target_steps.u);! h- |. n5 q! X5 L: W/ i# n
! |! ~) h) \; ]4 t0 q+ _
* ?/ c6 M( S/ [- B/ Y" _: J6 U

- U  J' c2 @9 k' n  E5 o5 |6 [  //what is our direction
* Q# ], _* a5 w: j5 d9 O1 N3 p  x_direction = (target_units.x >= current_units.x);$ Y) v, L1 C! f. H5 N
  y_direction = (target_units.y >= current_units.y);
' ], V$ n* B5 b+ L$ P/ }5 c  p/ R  z_direction = (target_units.z >= current_units.z);
/ ~" e) A9 @2 W7 _8 d  X' q  u_direction = (target_units.u >= current_units.u);
9 y% q# g+ G$ a$ F; Y7 R9 Z6 ]8 {- G  X8 ]3 D4 ?9 @4 f" [' K6 }: F
  //set our direction pins as well
8 \( k' C" \, _- g( o0 n  digitalWrite(X_DIR_PIN,x_direction);" H) z$ Q1 X- N3 q( }
  digitalWrite(Y_DIR_PIN,y_direction);
7 O( K6 J" ?4 o: u% e# x  digitalWrite(Z_DIR_PIN,z_direction);
( ^5 Y) Z# |, W$ R  digitalWrite(U_DIR_PIN,u_direction); 1 e/ U; V0 w, N9 r' y
1 m! L5 d' a. k2 Q
  //绘制LCD
# N2 n9 V6 f, L* o" K$ _  LCD_DRAW();% E4 D' Z8 F# f

6 c' w% d& Y' r0 f}* e" l  }& f* ~7 @. P2 }" H
1 p+ n% X: v  a8 s5 f! {: T3 }- }6 K
: E" A7 e1 B' y9 T4 M6 B- _1 z& T" [
long calculate_feedrate_delay(float feedrate)
/ u9 n8 p& m/ h# r, \{
7 o* J1 `% {6 j  //how long is our line length?& W1 |0 y3 D: f, B* K
  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);. w! z$ {" F9 C5 T
  long master_steps = 0;, G7 _& Q4 ~9 r- Y8 T- H
( W  S, @' V5 F0 g4 K' U8 p2 B7 _% [
master_steps=(delta_steps.x > delta_steps.y)?delta_steps.x:delta_steps.y;
3 C9 D) o, |- Kmaster_steps=(delta_steps.z>master_steps)?delta_steps.z:master_steps;6 e0 c: ^( j1 {+ }+ [
master_steps=(delta_steps.u>master_steps)?delta_steps.u:master_steps;
% `/ `8 Q$ j4 A4 [8 J& J
4 X/ g% C  O; \6 c) @5 R9 |% T
' ^5 \3 o6 i6 E3 V+ a: s' A: D% Y0 Q8 `( o8 \( \2 o) _
( |$ c  P2 Z6 z% O$ o4 x0 g, P
  //calculate delay between steps in microseconds.  this is sort of tricky, but not too bad.7 F$ l9 q% S* i+ w* w
  //the formula has been condensed to save space.  here it is in english:1 @. n) l3 U( m( B7 u7 q' G
  // distance / feedrate * 60000000.0 = move duration in microseconds
' Y8 h- J6 A* F+ v$ s( x  // move duration / master_steps = time between steps for master axis.
8 ^* F  v, T3 m# C/ _9 L return ((distance * 6000000.0) / feedrate) / master_steps;- G! y9 Y+ \5 ^& ~- K* l6 U
: M, P  V) a' d9 b2 F5 P  y4 Q
}- u/ b; \$ \, U

, G( ]5 g4 }9 q9 h' Ylong getMaxSpeed()! R; y" Z1 a4 n  E
{2 \8 m" t& A* z8 G3 |7 z2 M# `
if (delta_steps.z > 0 || delta_steps.u > 0 )
2 x3 \" E: |: S2 d! T    return calculate_feedrate_delay(FAST_Z_FEEDRATE);
3 \+ ]: D( ^/ x; C  else
: _! ?2 |" K- Z  j- o* j1 d1 f    return calculate_feedrate_delay(FAST_XY_FEEDRATE);" _- O0 ?  X7 E% r6 c% w
}
2 _6 A9 d! Z0 h+ f, Q7 q
: `/ m% v, M1 U, t; e6 Y3 g4 ]void disable_steppers()5 q/ n/ ?) Q9 E7 Q
{
6 P4 [' R* F# l4 z  D; r  //enable our steppers
! b3 J9 E" s: L" H; f, B  digitalWrite(X_ENABLE_PIN, LOW);, m( {- u, P/ h  c  T* j# u
  digitalWrite(Y_ENABLE_PIN, LOW);
+ ~% A" `$ m# ?  digitalWrite(Z_ENABLE_PIN, LOW);& t6 O: @6 H# g& R9 J7 `# g
  digitalWrite(U_ENABLE_PIN, LOW);$ k4 {$ K' v( z- I! S
}- ], m5 J6 S7 g* e; g
. ^" m9 T0 {, ]6 a% @7 z
& V7 m! _8 z" C, O' h1 f7 B
//绘制LCD
, T; A" z, n, v* j/ }& D; F//unsigned int DRAWCount=0; # n6 f; `, e4 a# i1 G" C$ Q
void LCD_DRAW()
, ?) i& j0 k8 b; }{
" T" k9 ?- ^& R0 n! F5 \& r6 k    lcd.clear();
: ?# O9 F" y: d. l; p+ V3 x    lcd.setCursor(0, 0);# m9 F7 c- a% g
    lcd.print("    X=");) J/ Z/ E: R6 n; F$ ?1 b, ^
    lcd.print(current_units.x);
& s4 J2 b9 U. \% {1 {    lcd.setCursor(0, 1);: m1 s$ B1 ?& y/ R
    lcd.print("    Y=");! i0 D/ }, I3 o4 x& }, B8 q& o
    lcd.print(current_units.y);: l+ P/ t2 m, s# F8 n* C
    lcd.setCursor(0, 2);1 e) O5 ^" ]$ t: d" x9 _
    lcd.print("Z=");% y+ }) Z' a. f7 }/ T' u
    lcd.print(current_units.z);* v* Q  I9 d" E, a7 I: C
    lcd.setCursor(0, 3);7 G2 F( X2 }  a
    lcd.print("U=");2 n1 A( e" O- D9 C: K0 ]" i
    lcd.print(current_units.u);& ]7 M  z! g2 w: v' ~
  }% o9 y. B3 C' G* `5 [
// else if (DRAWCount>=30)DRAWCount=0;) {) F4 f$ z8 p: `) B# j
//  else DRAWCount++;
+ T1 k/ O9 g6 j5 t. P. i8 D void SPEEN()
5 A2 t, e4 L& H# a4 _ {6 Z! {4 w- a* V( m( f
   delayMicroseconds(analogRead(SPEEN_CTL)+1);
0 \8 x" j% x4 t/ ^" `  U9 F' ^ }0 P; W- O% p1 V8 Y8 I

7 u4 a" D6 ~- @. V. _
7 M3 q4 E" x! X6 T) A //delayMicroseconds(analogRead(SPEEN_CTL)+1);, V6 z$ D& t3 G5 m) |  h
//if (digitalRead(BUTTON_SP_EN)) SPEEN();
$ i+ G0 e& K) t2 x* D# k' s) C& i) F( K% Y* C8 c9 S& s/ d4 `
5 p- I9 @* e8 W9 p- F
回复 支持 反对

使用道具 举报

 楼主| 发表于 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:52 , Processed in 0.092517 second(s), 15 queries , Gzip On.

Powered by Discuz! X3.4 Licensed

© 2001-2017 Comsenz Inc.

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