运动控制入门篇GCode_Interpreter
本帖最后由 xmdesign 于 2014-5-10 09:42 编辑首先声明
我不是专家，业余捣鼓
源代码首先来自网络
开发平台Arduino及Maple
http://www.arduino.cc/
http://leaflabs.com/devices/
国内活跃必威体育网址
http://www.geek-workshop.com/forum.php
竟然缺少积累，首先寻找最大的开放式开源平台，这样可以积累全球范围的创客和DIY者的脑力活动经验
来入门了解熟悉思路架构等
我只捣鼓了下8位机Arduino 移植到32位机Maple 方面一点点事情，
许多功能还木完成，不过作为低档次得应用可能还有可能
我自己也是个半桶水，再乐意玩的起码自学能力还是要有点吧
拒绝 所有的求
求人不如求自己 不然木玩
高手绕道 谢谢！
本帖最后由 xmdesign 于 2014-5-10 09:52 编辑
GCode_Interpreter是比较容易懂的，木那些寄存器等虾米开始不容易懂的东东
贴代码先
直接Maple的，某宝许多超便宜哈
晕，我怎么上不了RAR?
想玩的留下 e妹吧
第一个妹麻烦传第二个妹哈
我平常杂事多
谁放网盘上再，麻烦开放下 // Arduino G-code Interpreter for Rep Rap
// v1.0 by Mike Ellery - initial software (mellery@gmail.com)
// v1.1 by Zach Hoeken - cleaned up and did lots of tweaks (hoeken@gmail.com)
// v1.2 by Chris Meighan - cleanup / G2&G3 support (cmeighan@gmail.com)
// v1.3 by Zach Hoeken - added thermocouple support and multi-sample temp readings. (hoeken@gmail.com)
// Arduino G-code Interpreter for Macro / Micro photography
// v1.4 by Gene Cooper - modified and setup controls for macro / micro photography (gene@fourchambers.org)
//modified by YaoHan China 2010.2.15
//modified by JiWei China 2010.2.22 (jwdesign@163.com,QQ:75990175)
#include <stdlib.h>
#include <LiquidCrystal.h>
//启动(高电平有效)/暂停(低电平有效)默认高电平
#define BUTTON_CTL 35
//点动模式键（高电平有效）默认低电平
#define BUTTON_SS 36
//点动键
#define BUTTON_MAN 15
//电位器速度控制
#define SPEEN_CTL 16
//手动调速使能
#define BUTTON_SP_EN 17
//LCD 类型 1604或1602
#define LCD_TYPE 1604
//LCD 引脚定义
#define LCD_RS 23
#define LCD_EN 24
#define LCD_D4 25
#define LCD_D5 26
#define LCD_D6 27
#define LCD_D7 28
//命令字符串
#define COMMAND_SIZE 128
char word_old;
byte serial_count=0;
int no_data = 0;
//LCD 引脚链接 配置
LiquidCrystal lcd(LCD_RS,LCD_EN,LCD_D4,LCD_D5,LCD_D6,LCD_D7);
//停止控制，高电平有效
#define BUTTON_STOP 30
int stop_flag=0;
//暂停
void pause(){
while(!digitalRead(BUTTON_CTL));
}
void stopper(){
delayMicroseconds(10);
stop_flag = digitalRead(BUTTON_STOP);
}
void setup()
{
//show start
SerialUSB.println("start");
//启动lcd
lcd.begin(16,4);
lcd.setCursor(0, 0);
lcd.print("hello, world!");
//初始化控制引脚及引脚功能
pinMode(BUTTON_CTL,INPUT_PULLUP);
pinMode(BUTTON_SS,INPUT_PULLDOWN);
pinMode(BUTTON_STOP,INPUT_PULLDOWN);
pinMode(BUTTON_MAN,INPUT_ANALOG);
//控制引脚的中断
attachInterrupt(BUTTON_CTL,pause,FALLING);//暂停的中断
attachInterrupt(BUTTON_STOP,stopper,CHANGE);
//other init
init_process_string();
init_steppers();
init_camera();
}
void loop()
{
char c;
//读取输入的字符
if ((SerialUSB.available() > 0) && (!stop_flag))
{
c = SerialUSB.read();
no_data = 0;
//换行符代表一个命令的结束
if (c != '\n')
{
word_old = c;
serial_count++;
}
}
//标记没有数据输入
else
{
no_data++;
delayMicroseconds(100);
}
//if theres a pause or we got a real command, do it
if ((serial_count && (c == '\n' || no_data > 100)) && (!stop_flag))
{
//处理命令
process_string(word_old, serial_count);
//清除命令
init_process_string();
}
//如果没有数据关闭电机
if (no_data > 1000)
disable_steppers();
//如果ss键按下进入点动模式
//if(digitalRead(BUTTON_SS)) ss();
run_a();
}
//点动模式
void ss(){
delay(1);
if(!digitalRead(BUTTON_SS))return;
if(!digitalRead(BUTTON_SS))return;
//init_process_string();
//init_steppers();
//init_camera();
// SerialUSB.println("Step By Step Mode");
//#if (LCD_TYPE == 1604 )
//当LCD为1604时显示要处理的命令
lcd.setCursor(0, 0);
lcd.print("Step By Step Mode");
//#endif
process_string("G1 F5000",8);
process_string("G91",8);
//init_process_string();
while(digitalRead(BUTTON_SS)){
int i=0;
i=analogRead(BUTTON_MAN)>>9;
//if (i==0){break;}
//SerialUSB.println(i);
//delay(1000);
if(i==2){
set_target(1000.0, 0.0, 0.0, 0.0);
dda_move(getMaxSpeed());
if(stop_flag) return;
//process_string("X0.01",5);
//init_process_string();
}
else
if(i==5){
set_target(-1000.0, 0.0, 0.0, 0.0);
dda_move(getMaxSpeed());
if(stop_flag) return;
//process_string("X-0.01",6);
//init_process_string();
}
}
//init_steppers();
//init_camera();
// SerialUSB.println("Return To Normal Mode");
process_string("G1",8);
// process_string("G91",8);
init_process_string();
//#if (LCD_TYPE == 1604 )
//当LCD为1604时显示要处理的命令
// lcd.setCursor(0, 4);
// lcd.print("Return To Normal Mode");
//#endif
}
void run_a()
{
//delay(1);
//if(digitalRead(14)== HIGH)return;
//if(digitalRead(14)== HIGH)return;
//process_string("G1 F2000",8);
//process_string("G92",8);
process_string("G90",8);
process_string("X120 F10",5);
process_string("G4P2000",8);
process_string("X10 F10",5);
// process_string("M101",8);
// process_string("X-50",5);
dda_move(getMaxSpeed());
init_process_string();
}
// 定义机器参数
#define X_STEPS_PER_INCH 400
#define X_STEPS_PER_MM 16.0
#define X_MOTOR_STEPS 200
#define Y_STEPS_PER_INCH 400.0
#define Y_STEPS_PER_MM 16.0
#define Y_MOTOR_STEPS 200
#define Z_STEPS_PER_INCH 400.0
#define Z_STEPS_PER_MM 16.0
#define Z_MOTOR_STEPS 200
#define U_STEPS_PER_INCH 400.0
#define U_STEPS_PER_MM 16.0
#define U_MOTOR_STEPS 200
//最大进给率
#define FAST_XY_FEEDRATE 1500.0
#define FAST_Z_FEEDRATE1500.0
#define FAST_U_FEEDRATE1500.0
// Units in curve section
#define CURVE_SECTION_INCHES 0.019685
#define CURVE_SECTION_MM 0.5
// Set to one if sensor outputs inverting (ie: 1 means open, 0 means closed)
// RepRap opto endstops are *not* inverting.
#define SENSORS_INVERTING 1
/****************************************************************************************
* digital i/o pin assignment
*
* this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5
****************************************************************************************/
//camera shutter and control pins
#define CAM_SHUTTER_PIN1 29
#define CAM_SHUTTER_PIN2 30
//#define CAM_AUX_PIN1 31 // analog 0
//#define CAM_AUX_PIN2 32 // analog 1
//#define CAM_AUX_PIN3 33 // analog 2
//#define CAM_AUX_PIN4 34 // analog 3
// stepper driver pins
#define X_STEP_PIN 7
#define X_DIR_PIN 8
#define X_ENABLE_PIN 19
#define Y_STEP_PIN 9
#define Y_DIR_PIN 10
#define Y_ENABLE_PIN 19
#define Z_STEP_PIN 11
#define Z_DIR_PIN 12
#define Z_ENABLE_PIN 19
#define U_STEP_PIN 13
#define U_DIR_PIN 14
#define U_ENABLE_PIN 19
// limits not used right now
#define X_MIN_PIN 14
#define X_MAX_PIN 14
#define Y_MIN_PIN 14
#define Y_MAX_PIN 14
#define Z_MIN_PIN 14
#define Z_MAX_PIN 14
#define U_MIN_PIN 14
#define U_MAX_PIN 14
void init_camera()
{
pinMode(CAM_SHUTTER_PIN1, OUTPUT);
pinMode(CAM_SHUTTER_PIN2, OUTPUT);
//pinMode(CAM_AUX_PIN1, OUTPUT); // analog 0
// pinMode(CAM_AUX_PIN2, OUTPUT); // analog 1
// pinMode(CAM_AUX_PIN3, OUTPUT); // analog 2
// pinMode(CAM_AUX_PIN4, OUTPUT); // analog 3
}
void camera_shutter1()
{
// fire the camera shutter via relay...1/4 sec hold time
digitalWrite(CAM_SHUTTER_PIN1, HIGH);
delay(250);
digitalWrite(CAM_SHUTTER_PIN1, LOW);
}
void camera_shutter2()
{
// fire the camera shutter via relay...1/4 sec hold time
digitalWrite(CAM_SHUTTER_PIN2, HIGH);
delay(250);
digitalWrite(CAM_SHUTTER_PIN2, LOW);
}
/*
void camera_aux1_on()
{
// turn aux relay 1 on
digitalWrite(CAM_AUX_PIN1, HIGH);
}
void camera_aux1_off()
{
// turn aux relay 1 off
digitalWrite(CAM_AUX_PIN1, LOW);
}
void camera_aux2_on()
{
// turn aux relay 2 on
digitalWrite(CAM_AUX_PIN2, HIGH);
}
void camera_aux2_off()
{
// turn aux relay 2 off
digitalWrite(CAM_AUX_PIN2, LOW);
}
void camera_aux3_on()
{
// turn aux relay 3 on
digitalWrite(CAM_AUX_PIN3, HIGH);
}
void camera_aux3_off()
{
// turn aux relay 3 off
digitalWrite(CAM_AUX_PIN3, LOW);
}
void camera_aux4_on()
{
// turn aux relay 4 on
digitalWrite(CAM_AUX_PIN4, HIGH);
}
void camera_aux4_off()
{
// turn aux relay 4 off
digitalWrite(CAM_AUX_PIN4, LOW);
} 楼主推荐的网址不错
请问楼主是玩什么的，用乐高玩具吗？ // our point structure to make things nice.
struct LongPoint {
long x;
long y;
long z;
long u;
};
struct FloatPoint {
float x;
float y;
float z;
float u;
};
FloatPoint current_units;
FloatPoint target_units;
FloatPoint delta_units;
FloatPoint current_steps;
FloatPoint target_steps;
FloatPoint delta_steps;
boolean abs_mode = false; //0 = 增量位置模式; 1 = 绝对位置模式
//default to inches for units
float x_units = X_STEPS_PER_INCH;
float y_units = Y_STEPS_PER_INCH;
float z_units = Z_STEPS_PER_INCH;
float u_units = U_STEPS_PER_INCH;
float curve_section = CURVE_SECTION_INCHES;
//our direction vars
byte x_direction = 1;
byte y_direction = 1;
byte z_direction = 1;
byte u_direction = 1;
//初始化字符串处理
void init_process_string()
{
//init our command
for (byte i=0; i<COMMAND_SIZE; i++)
word_old = 0;
serial_count = 0;
}
//our feedrate variables.
float feedrate = 0.0;
long feedrate_micros = 0;
//读取并执行命令
void process_string(char instruction[], int size)
{
//the character / means delete block... used for comments and stuff.
if (instruction == '/')
{
// SerialUSB.print("ok");
// SerialUSB.print(byte(78));
return;
}
//init baby!
FloatPoint fp;
fp.x = 0.0;
fp.y = 0.0;
fp.z = 0.0;
fp.u = 0.0;
byte code = 0;
//显示在处理的命令
#if (LCD_TYPE == 1604 )
// lcd.setCursor(0, 4);
// lcd.print(word_old);
#endif
SerialUSB.println();
SerialUSB.print(instruction);
SerialUSB.print("\t");
//what line are we at?
// long line = -1;
// if (has_command('N', instruction, size))
// line = (long)search_string('N', instruction, size);
/*
Serial.print("line: ");
Serial.println(line);
Serial.println(instruction);
*/
//判断是否读取了个 G代码?
if (
has_command('G', instruction, size) ||
has_command('X', instruction, size) ||
has_command('Y', instruction, size) ||
has_command('Z', instruction, size) ||
has_command('U', instruction, size)
)
{
//which one?
code = (int)search_string('G', instruction, size);
// Get co-ordinates if required by the code type given
switch (code)
{
case 0:
case 1:
case 2:
case 3:
if(abs_mode)
{
//we do it like this to save time. makes curves better.
//eg. if only x and y are specified, we dont have to waste time looking up z.
if (has_command('X', instruction, size))
fp.x = search_string('X', instruction, size);
else
fp.x = current_units.x;
if (has_command('Y', instruction, size))
fp.y = search_string('Y', instruction, size);
else
fp.y = current_units.y;
if (has_command('Z', instruction, size))
fp.z = search_string('Z', instruction, size);
else
fp.z = current_units.z;
if (has_command('U', instruction, size))
fp.u = search_string('U', instruction, size);
else
fp.u = current_units.u;
}
else
{
fp.x = search_string('X', instruction, size) + current_units.x;
fp.y = search_string('Y', instruction, size) + current_units.y;
fp.z = search_string('Z', instruction, size) + current_units.z;
fp.u = search_string('U', instruction, size) + current_units.u;
}
break;
}
//do something!
switch (code)
{
//Rapid Positioning
//Linear Interpolation
//these are basically the same thing.
case 0:
case 1:
//set our target.
set_target(fp.x, fp.y, fp.z, fp.u);
//set_targeta( fp.a);
//do we have a set speed?
if (has_command('G', instruction, size))
{
//adjust if we have a specific feedrate.
if (code == 1)
{
//how fast do we move?
feedrate = search_string('F', instruction, size);
if (feedrate > 0)
feedrate_micros = calculate_feedrate_delay(feedrate);
//nope, no feedrate
else
feedrate_micros = getMaxSpeed();
}
//use our max for normal moves.
else
feedrate_micros = getMaxSpeed();
}
//nope, just coordinates!
else
{
//do we have a feedrate yet?
if (feedrate > 0)
feedrate_micros = calculate_feedrate_delay(feedrate);
//nope, no feedrate
else
feedrate_micros = getMaxSpeed();
}
//finally move.
dda_move(feedrate_micros);
if(stop_flag) return;
break;
//Clockwise arc
case 2:
//Counterclockwise arc
case 3:
FloatPoint cent;
// Centre coordinates are always relative
cent.x = search_string('I', instruction, size) + current_units.x;
cent.y = search_string('J', instruction, size) + current_units.y;
float angleA, angleB, angle, radius, length, aX, aY, bX, bY;
aX = (current_units.x - cent.x);
aY = (current_units.y - cent.y);
bX = (fp.x - cent.x);
bY = (fp.y - cent.y);
if (code == 2) { // Clockwise
angleA = atan2(bY, bX);
angleB = atan2(aY, aX);
}
else { // Counterclockwise
angleA = atan2(aY, aX);
angleB = atan2(bY, bX);
}
// Make sure angleB is always greater than angleA
// and if not add 2PI so that it is (this also takes
// care of the special case of angleA == angleB,
// ie we want a complete circle)
if (angleB <= angleA) angleB += 2 * M_PI;
angle = angleB - angleA;
radius = sqrt(aX * aX + aY * aY);
length = radius * angle;
int steps, s, step;
steps = (int) ceil(length / curve_section);
FloatPoint newPoint;
for (s = 1; s <= steps; s++) {
step = (code == 3) ? s : steps - s; // Work backwards for CW
newPoint.x = cent.x + radius * cos(angleA + angle * ((float) step / steps));
newPoint.y = cent.y + radius * sin(angleA + angle * ((float) step / steps));
set_target(newPoint.x, newPoint.y, fp.z, fp.u);
// Need to calculate rate for each section of curve
if (feedrate > 0)
feedrate_micros = calculate_feedrate_delay(feedrate);
else
feedrate_micros = getMaxSpeed();
// Make step
dda_move(feedrate_micros);
if(stop_flag) return;
}
break;
//Dwell
case 4:
delay((int)search_string('P', instruction, size));
break;
//Inches for Units
case 20:
x_units = X_STEPS_PER_INCH;
y_units = Y_STEPS_PER_INCH;
z_units = Z_STEPS_PER_INCH;
u_units = U_STEPS_PER_INCH;
curve_section = CURVE_SECTION_INCHES;
calculate_deltas();
break;
//mm for Units
case 21:
x_units = X_STEPS_PER_MM;
y_units = Y_STEPS_PER_MM;
z_units = Z_STEPS_PER_MM;
u_units = U_STEPS_PER_MM;
curve_section = CURVE_SECTION_MM;
calculate_deltas();
break;
//go home.
case 28:
set_target(0.0, 0.0, 0.0, 0.0);
dda_move(getMaxSpeed());
if(stop_flag) return;
break;
//go home via an intermediate point.
case 30:
fp.x = search_string('X', instruction, size);
fp.y = search_string('Y', instruction, size);
fp.z = search_string('Z', instruction, size);
fp.u = search_string('U', instruction, size);
//set our target.
if(abs_mode)
{
if (!has_command('X', instruction, size))
fp.x = current_units.x;
if (!has_command('Y', instruction, size))
fp.y = current_units.y;
if (!has_command('Z', instruction, size))
fp.z = current_units.z;
if (!has_command('U', instruction, size))
fp.u = current_units.u;
set_target(fp.x, fp.y, fp.z, fp.u);
}
else
set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.u + fp.u );
//go there.
dda_move(getMaxSpeed());
if(stop_flag) return;
//go home.
set_target(0.0, 0.0, 0.0, 0.0 );
dda_move(getMaxSpeed());
if(stop_flag) return;
break;
//Absolute Positioning
case 90:
abs_mode = true;
break;
//Incremental Positioning
case 91:
abs_mode = false;
break;
//Set as home
case 92:
set_position(0.0, 0.0, 0.0, 0.0 );
break;
/*
//Inverse Time Feed Mode
case 93:
break;//TODO: add this
//Feed per Minute Mode
case 94:
break;//TODO: add this
*/
default:
SerialUSB.print("huh? G");
SerialUSB.println(code,DEC);
}
}
//find us an m code.
if (has_command('M', instruction, size))
{
code = search_string('M', instruction, size);
switch (code)
{
//TODO: this is a bug because search_string returns 0.gotta fix that.
case 0:
true;
break;
case 100:
break;
// fire camera relay
case 101:
camera_shutter1();
break;
// fire camera relay2
case 102:
camera_shutter2();
break;
/*
// turn aux 1 relay on
case 103:
camera_aux1_on();
break;
// turn aux 1 relay off
case 104:
camera_aux1_off();
break;
// turn aux 2 relay on
case 105:
camera_aux2_on();
break;
// turn aux 2 relay off
case 106:
camera_aux2_off();
break;
// turn aux 3 relay on
case 107:
camera_aux3_on();
break;
// turn aux 3 relay off
case 108:
camera_aux3_off();
break;
// turn aux 4 relay on
case 109:
camera_aux4_on();
break;
// turn aux 4 relay off
case 110:
camera_aux4_off();
break;
*/
default:
SerialUSB.print("Huh? M");
SerialUSB.println(code);
}
}
//tell our host we're done.
SerialUSB.print(byte(78));
}
//look for the number that appears after the char key and return it
double search_string(char key, char instruction[], int string_size)
{
char temp = " ";
for (byte i=0; i<string_size; i++)
{
if (instruction == key)
{
i++;
int k = 0;
while (i < string_size && k < 10)
{
if (instruction == 0 || instruction == ' ')
break;
temp = instruction;
i++;
k++;
}
return strtod(temp, NULL);
}
}
return 0;
}
//look for the command if it exists.
bool has_command(char key, char instruction[], int string_size)
{
for (byte i=0; i<string_size; i++)
{
if (instruction == key){
return true;
}
}
return false;
}
//init our variables
long max_delta;
long x_counter;
long y_counter;
long z_counter;
long u_counter;
long x_pos = 0; // x position in terms of absoloute motor stepps
long y_pos = 0; // y position in terms of absoloute motor stepps
long z_pos = 0; // z position in terms of absoloute motor stepps
long u_pos = 0; // U position in terms of absoloute motor stepps
bool x_can_step;
bool y_can_step;
bool z_can_step;
bool u_can_step;
int milli_delay;
void init_steppers()
{
//turn them off to start.
disable_steppers();
//init our points.
current_units.x = 0.0;
current_units.y = 0.0;
current_units.z = 0.0;
current_units.u = 0.0;
target_units.x = 0.0;
target_units.y = 0.0;
target_units.z = 0.0;
target_units.u = 0.0;
pinMode(X_STEP_PIN, OUTPUT);
pinMode(X_DIR_PIN, OUTPUT);
pinMode(X_ENABLE_PIN, OUTPUT);
pinMode(X_MIN_PIN, INPUT);
pinMode(X_MAX_PIN, INPUT);
pinMode(Y_STEP_PIN, OUTPUT);
pinMode(Y_DIR_PIN, OUTPUT);
pinMode(Y_ENABLE_PIN, OUTPUT);
pinMode(Y_MIN_PIN, INPUT);
pinMode(Y_MAX_PIN, INPUT);
pinMode(Z_STEP_PIN, OUTPUT);
pinMode(Z_DIR_PIN, OUTPUT);
pinMode(Z_ENABLE_PIN, OUTPUT);
pinMode(Z_MIN_PIN, INPUT);
pinMode(Z_MAX_PIN, INPUT);
pinMode(U_STEP_PIN, OUTPUT);
pinMode(U_DIR_PIN, OUTPUT);
pinMode(U_ENABLE_PIN, OUTPUT);
pinMode(U_MIN_PIN, INPUT);
pinMode(U_MAX_PIN, INPUT);
//figure our stuff.
calculate_deltas();
}
void dda_move(long micro_delay)
{
//enable our steppers
digitalWrite(X_ENABLE_PIN, HIGH);
digitalWrite(Y_ENABLE_PIN, HIGH);
digitalWrite(Z_ENABLE_PIN, HIGH);
digitalWrite(U_ENABLE_PIN, HIGH);
//figure out our deltas
max_delta = max(delta_steps.x, delta_steps.y);
max_delta = max(delta_steps.z, max_delta);
max_delta = max(delta_steps.u, max_delta);
//init stuff.
long x_counter = -max_delta/2;
long y_counter = -max_delta/2;
long z_counter = -max_delta/2;
long u_counter = -max_delta/2;
//our step flags
bool x_can_step = 0;
bool y_can_step = 0;
bool z_can_step = 0;
bool u_can_step = 0;
if (micro_delay >= 16383)
milli_delay = micro_delay / 1000;
else
milli_delay = 0;
//do our DDA line!
do
{
if(( digitalRead(BUTTON_SS)?analogRead(BUTTON_MAN)>>9==0:0) || stop_flag) break;
x_can_step = can_step(X_MIN_PIN, X_MAX_PIN, current_steps.x, target_steps.x, x_direction);
y_can_step = can_step(Y_MIN_PIN, Y_MAX_PIN, current_steps.y, target_steps.y, y_direction);
z_can_step = can_step(Z_MIN_PIN, Z_MAX_PIN, current_steps.z, target_steps.z, z_direction);
u_can_step = can_step(U_MIN_PIN, U_MAX_PIN, current_steps.u, target_steps.u, u_direction);
if (x_can_step)
{
x_counter += delta_steps.x;
if (x_counter > 0)
{
do_step(X_STEP_PIN);
x_counter -= max_delta;
if (x_direction)
{ current_steps.x++; x_pos++; }
else
{ current_steps.x--; x_pos--; }
}
}
if (y_can_step)
{
y_counter += delta_steps.y;
if (y_counter > 0)
{
do_step(Y_STEP_PIN);
y_counter -= max_delta;
if (y_direction)
{ current_steps.y++; y_pos++; }
else
{ current_steps.y--; y_pos--; }
}
}
if (z_can_step)
{
z_counter += delta_steps.z;
if (z_counter > 0)
{
do_step(Z_STEP_PIN);
z_counter -= max_delta;
if (z_direction)
{ current_steps.z++; z_pos++; }
else
{ current_steps.z--; z_pos--; }
}
}
if (u_can_step)
{
u_counter += delta_steps.u;
if (u_counter > 0)
{
do_step(U_STEP_PIN);
u_counter -= max_delta;
if (u_direction)
{ current_steps.u++; u_pos++; }
else
{ current_steps.u--; u_pos--; }
}
}
//wait for next step.
if (milli_delay > 0){
//if (digitalRead(BUTTON_SP_EN)) SPEEN();
delay(milli_delay);
}
else{
//if (digitalRead(BUTTON_SP_EN)) SPEEN();
if(micro_delay>0)delayMicroseconds(micro_delay);
}
//if(x_can_step%40 || y_can_step%40 || z_can_step%40);
}
while (x_can_step || y_can_step || z_can_step || u_can_step);
//set our points to be the same
current_units.x = (float) x_pos / X_STEPS_PER_INCH;
current_units.y = (float) y_pos / Y_STEPS_PER_INCH;
current_units.z = (float) z_pos / Z_STEPS_PER_INCH;
current_units.u = (float) u_pos / U_STEPS_PER_INCH;
set_position(current_units.x, current_units.y, current_units.z, current_units.u );
long x_pos = 0; // x position in terms of absoloute motor stepps
long y_pos = 0; // y position in terms of absoloute motor stepps
long z_pos = 0; // z position in terms of absoloute motor stepps
long u_pos = 0; // u position in terms of absoloute motor stepps
calculate_deltas();
}
bool can_step(byte min_pin, byte max_pin, long current, long target, byte direction)
{
//stop us if we're on target
if (target == current)
return false;
//stop us if we're at home and still going
else if (read_switch(min_pin) && !direction)
return false;
//stop us if we're at max and still going
else if (read_switch(max_pin) && direction)
return false;
//default to being able to step
return true;
}
void do_step(byte step_pin)
{
digitalWrite(step_pin, HIGH);
//delayMicroseconds(1);
digitalWrite(step_pin, LOW);
}
bool read_switch(byte pin)
{
//dual read as crude debounce
if ( SENSORS_INVERTING )
return !digitalRead(pin) && !digitalRead(pin);
else
return digitalRead(pin) && digitalRead(pin);
}
long to_steps(float steps_per_unit, float units)
{
return steps_per_unit * units;
}
void set_target(float x, float y, float z, float u)
{
target_units.x = x;
target_units.y = y;
target_units.z = z;
target_units.u = u;
calculate_deltas();
}
void set_position(float x, float y, float z, float u)
{
current_units.x = x;
current_units.y = y;
current_units.z = z;
current_units.u = u;
calculate_deltas();
}
void calculate_deltas()
{
//figure our deltas.
delta_units.x = (target_units.x >= current_units.x) ? (target_units.x - current_units.x) : (current_units.x - target_units.x);
delta_units.y = (target_units.y >= current_units.y) ? (target_units.y - current_units.y) : (current_units.y - target_units.y);
delta_units.z = (target_units.z >= current_units.z) ? (target_units.z - current_units.z) : (current_units.z - target_units.z);
delta_units.u = (target_units.u >= current_units.u) ? (target_units.u - current_units.u) : (current_units.u - target_units.u);
//set our steps current, target, and delta
current_steps.x = to_steps(x_units, current_units.x);
current_steps.y = to_steps(y_units, current_units.y);
current_steps.z = to_steps(z_units, current_units.z);
current_steps.u = to_steps(u_units, current_units.u);
target_steps.x = to_steps(x_units, target_units.x);
target_steps.y = to_steps(y_units, target_units.y);
target_steps.z = to_steps(z_units, target_units.z);
target_steps.u = to_steps(u_units, target_units.u);
delta_steps.x = (target_steps.x >= current_steps.x) ? (target_steps.x - current_steps.x) : (current_steps.x - target_steps.x);
delta_steps.y = (target_steps.y >= current_steps.y) ? (target_steps.y - current_steps.y) : (current_steps.y - target_steps.y);
delta_steps.z = (target_steps.z >= current_steps.z) ? (target_steps.z - current_steps.z) : (current_steps.z - target_steps.z);
delta_steps.u = (target_steps.u >= current_steps.u) ? (target_steps.u - current_steps.u) : (current_steps.u - target_steps.u);
//what is our direction
x_direction = (target_units.x >= current_units.x);
y_direction = (target_units.y >= current_units.y);
z_direction = (target_units.z >= current_units.z);
u_direction = (target_units.u >= current_units.u);
//set our direction pins as well
digitalWrite(X_DIR_PIN,x_direction);
digitalWrite(Y_DIR_PIN,y_direction);
digitalWrite(Z_DIR_PIN,z_direction);
digitalWrite(U_DIR_PIN,u_direction);
//绘制LCD
LCD_DRAW();
}
long calculate_feedrate_delay(float feedrate)
{
//how long is our line length?
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);
long master_steps = 0;
master_steps=(delta_steps.x > delta_steps.y)?delta_steps.x:delta_steps.y;
master_steps=(delta_steps.z>master_steps)?delta_steps.z:master_steps;
master_steps=(delta_steps.u>master_steps)?delta_steps.u:master_steps;
//calculate delay between steps in microseconds.this is sort of tricky, but not too bad.
//the formula has been condensed to save space.here it is in english:
// distance / feedrate * 60000000.0 = move duration in microseconds
// move duration / master_steps = time between steps for master axis.
return ((distance * 6000000.0) / feedrate) / master_steps;
}
long getMaxSpeed()
{
if (delta_steps.z > 0 || delta_steps.u > 0 )
return calculate_feedrate_delay(FAST_Z_FEEDRATE);
else
return calculate_feedrate_delay(FAST_XY_FEEDRATE);
}
void disable_steppers()
{
//enable our steppers
digitalWrite(X_ENABLE_PIN, LOW);
digitalWrite(Y_ENABLE_PIN, LOW);
digitalWrite(Z_ENABLE_PIN, LOW);
digitalWrite(U_ENABLE_PIN, LOW);
}
//绘制LCD
//unsigned int DRAWCount=0;
void LCD_DRAW()
{
lcd.clear();
lcd.setCursor(0, 0);
lcd.print(" X=");
lcd.print(current_units.x);
lcd.setCursor(0, 1);
lcd.print(" Y=");
lcd.print(current_units.y);
lcd.setCursor(0, 2);
lcd.print("Z=");
lcd.print(current_units.z);
lcd.setCursor(0, 3);
lcd.print("U=");
lcd.print(current_units.u);
}
// else if (DRAWCount>=30)DRAWCount=0;
//else DRAWCount++;
void SPEEN()
{
delayMicroseconds(analogRead(SPEEN_CTL)+1);
}
//delayMicroseconds(analogRead(SPEEN_CTL)+1);
//if (digitalRead(BUTTON_SP_EN)) SPEEN();
基本原代码的大概是这样，我是断断续续捣鼓的，玩到最后版的，后面我会找出来先测试下过，再贴上：） 请楼主发一份给我，感谢！178354773@qq.com
页:
[1]
2