GCode_interpreter解读
博客专区 > YANG_G 的博客 > 博客详情
GCode_interpreter解读
YANG_G 发表于2个月前
GCode_interpreter解读
  • 发表于 2个月前
  • 阅读 8
  • 收藏 0
  • 点赞 0
  • 评论 0

腾讯云 十分钟定制你的第一个小程序>>>   

关于GCode_interpreter

下学期要做课设,要徒手画a0的图,准备做个A0的绘图仪@_@

        

    本来想用grbl来驱动,但了解之后发现它是用avr的c写的,而且代码量也挺大的,考虑到后面可能要加一些功能(换笔什么的),怕修改不来,就放弃了。后来又找了很多g代码解释器,最后发现了GCode_interpreter。GCode_interpreter是用Arduino语言写的,理论上支持所有的arduino,虽然性能和功能上不及grbl,但是结构简单,代码量少。唯一比较不好的就是没有加减速功能,容易导致步进电机过冲或者丢步。

程序说明

        GCode_interpreter主要由_init、GCode_interpreter、process_string、stepper_contorl四个文件组成(当然还有extruder,但是这里就不做解释了)_init文件是对一些基础参数的预定义,比如步进电机分辨率、针脚等;GCode_interpreter是主程序;process_string存放一些和指令处理有关的函数;stepper_contorl存放和步进电机运动有关的函数。

    

process_string中的函数说明

函数 说明
init_process_string() 初始化串口数据
process_string() 处理指令
search_string() 提取指令中某个字符后空格前的浮点数
has_command() 判断指令中是否有某个字符

stepper_control中的函数说明

函数 说明
init_steppers() 初始化步进电机
dda_move() 执行指定运动
can_step() 判断步进电机是否可以运动
do_step 步进电机走一步
read_switch() 读取限位开关状态
to_step() 计算需要的脉冲数
set_target() 设定目标坐标
set_position() 设定当前坐标
caculate_deltas() 坐标计算(当前和目标的差值、当前已行走的步数、步数的差值、运动方向)
calculate_feedrate_delay() 计算两个脉冲之间的延迟
getMaxSpeed() 以最大速度运行(实为默认速度)

主要变量说明

变量 说明
worda[] 储存指令的数组
serial_count 指令长度
current_units.x(/y/z) 当前坐标
target_units.x(/y/z) 目标坐标
deltas_units.x(/y/z) 当前和目标坐标的差值
current_step.x(/y/z) 当前步数坐标
target_step.x(/y/z) 目标步数坐标
deltas_step.x(/y/z) 到达目标位置还需要行走的步数
abs_mode true为绝对坐标模式
x_units/y_units/z_units 步进电机运动1mm需要的脉冲数
x_direction/y_direction/z_direction 运动方向
feedrate 速度
feedrate_micros 脉冲间的延迟
x_can_step/y_can_step/z_can_step true允许运动

程序大致的流程图:

            

代码注释

以下代码和原固件有些地方不太一样(比如:原固件有些变量名在我的ide中和关键字冲突了,被我改了)

GCode_interpreter



// Arduino G-code Interpreter
// 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)
#include <HardwareSerial.h>
#include <Servo.h>//我在原固件基础上加入了m03和m05来控制舵机
//our command string
#define COMMAND_SIZE 128 //单个指令的最大长度
char worda[COMMAND_SIZE];//储存指令的数组
byte serial_count;//指令长度
int no_data = 0;//0为串口有数据,>0为无数据
Servo a;
void setup()
{
	//Do startup stuff here
	Serial.begin(115200);//波特率
	Serial.println("start");
	a.attach(12);//舵机
	//other initialization.
	init_process_string();//初始化
	init_steppers();
	init_extruder();
}

void loop()
{
	char c;
	
	//keep it hot!
	extruder_manage_temperature();//有关extruder的将不作注释

	//read in characters if we got them.
	if (Serial.available() > 0)//如果串口可用
	{
		c = Serial.read();//读取一个字符
		no_data = 0;//串口有数据
		
		//newlines are ends of commands.
		if (c != '\n')
		{
			worda[serial_count] = c;//在接收到换行之前将字符存入数组
			serial_count++;//指令长度累加
		}
	}
	//mark no data.
	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))//如果指令长度不为0,且接收到换行或短时间内串口无数据
	{
		//process our command!
		process_string(worda, serial_count);//执行指令

		//clear command.
		init_process_string();//初始化
	}

	//no data?  turn off steppers
	if (no_data > 1000)//长时间无数据
	  disable_steppers();//关闭步进电机
}

 

_init

//这个文件包含一些预定义
// define the parameters of our machine.
#define X_STEPS_PER_INCH 2540//走1英寸需要的脉冲数,以英寸为单位的时候使用,根据传动方式进行计算
#define X_STEPS_PER_MM   100//走1mm需要的脉冲数
#define X_MOTOR_STEPS    200//步进电机每转步数,固件中无需用到
//需要说明的是,一般步进驱动都有细分,一个脉冲实际上走的是:步/细分
//但是为了方便,后面都以一个脉冲一步来说明
#define Y_STEPS_PER_INCH 2540
#define Y_STEPS_PER_MM   100
#define Y_MOTOR_STEPS    200

#define Z_STEPS_PER_INCH 2540
#define Z_STEPS_PER_MM   100
#define Z_MOTOR_STEPS    200

//our maximum feedrates
#define FAST_XY_FEEDRATE 3000//XY轴的最大速度,实际上是默认速度
#define FAST_Z_FEEDRATE  36000

// Units in curve section
#define CURVE_SECTION_INCHES 0.005
#define CURVE_SECTION_MM 0.5//曲线片段长,在圆弧插补中使用,圆弧实际上是等分上的一些线段拟合的,


// RepRap opto endstops are *not* inverting.
#define SENSORS_INVERTING 0//限位状态,0为常开

// How many temperature samples to take.  each sample takes about 100 usecs.
#define TEMPERATURE_SAMPLES 5

/****************************************************************************************
* digital i/o pin assignment
*
* this uses the undocumented feature of Arduino - pins 14-19 correspond to analog 0-5
****************************************************************************************/
//以下是针脚的定义,根据自己的arduino配置
//cartesian bot pins
#define X_STEP_PIN 2 //脉冲
#define X_DIR_PIN 5  //方向
#define X_MIN_PIN 9 //限位
#define X_MAX_PIN 9
#define X_ENABLE_PIN 8 //使能

#define Y_STEP_PIN 3
#define Y_DIR_PIN 6
#define Y_MIN_PIN 10
#define Y_MAX_PIN 10
#define Y_ENABLE_PIN 8

#define Z_STEP_PIN 4
#define Z_DIR_PIN 7
#define Z_MIN_PIN 11
#define Z_MAX_PIN 11
#define Z_ENABLE_PIN 8

//extruder pins
#define EXTRUDER_MOTOR_SPEED_PIN   0
#define EXTRUDER_MOTOR_DIR_PIN     0
#define EXTRUDER_HEATER_PIN        0
#define EXTRUDER_FAN_PIN           0
#define EXTRUDER_THERMISTOR_PIN    0  //a -1 disables thermistor readings
#define EXTRUDER_THERMOCOUPLE_PIN  -1 //a -1 disables thermocouple readings

process_string

// our point structure to make things nice.
//定义两种数据结构,提高代码可读性
struct LongPoint {
	long x;
	long y;
 	long z;
};

struct FloatPoint {
	float x;
	float y;
 	float z;
};

FloatPoint current_units;
FloatPoint target_units;
FloatPoint delta_units;

FloatPoint current_steps;
FloatPoint target_steps;
FloatPoint delta_steps;

boolean abs_mode = true; //true开启绝对坐标模式

//default to inches for units
float x_units = X_STEPS_PER_MM;
float y_units = Y_STEPS_PER_MM;
float z_units = Z_STEPS_PER_MM;
float curve_section = CURVE_SECTION_MM;//原固件默认是以英寸为单位,我改成了mm

//our direction vars
byte x_direction = 1;
byte y_direction = 1;
byte z_direction = 1;//方向

int pre_feed=0;//这个是我加入的,储存的是前一次的移动速度,后面会讲解
//init our string processing
void init_process_string()//初始化指令
{
	//init our command
	for (byte i=0; i<COMMAND_SIZE; i++)//清零指令数组
		worda[i] = 0;
	serial_count = 0;
}

//our feedrate variables.
float feedrate = 0.0;//移动速度
long feedrate_micros = 0;//脉冲延迟

//Read the string and execute instructions
void process_string(char instruction[], int sizea)//处理指令
{
	//the character / means delete block... used for comments and stuff.
	if (instruction[0] == '/')//接收到指令的结束符
	{
		Serial.println("ok");
		return;
	}

	//init baby!
	FloatPoint fp;
	fp.x = 0.0;
	fp.y = 0.0;
	fp.z = 0.0;//目标位置(这里是中间变量)

	byte code = 0;
  
	 
//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);
*/		
	//did we get a gcode?
	if (
		has_command('G', instruction, sizea) ||
		has_command('X', instruction, sizea) ||
		has_command('Y', instruction, sizea) ||
		has_command('Z', instruction, sizea)
	)//接收到的是否是G代码(含有G、X、Y、Z字符)
	{
		//which one?
		code = (int)search_string('G', instruction, sizea);//判断G代码的类型
		
		// Get co-ordinates if required by the code type given
		switch (code)
		{
			case 0:
			case 1:
			case 2:
			case 3: //G0、G1、G2、G3都进行指令参数的提取
				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, sizea))
						fp.x = search_string('X', instruction, sizea);
					else
						fp.x = current_units.x;
				
					if (has_command('Y', instruction, sizea))
						fp.y = search_string('Y', instruction, sizea);
					else
						fp.y = current_units.y;
				
					if (has_command('Z', instruction, sizea))
						fp.z = search_string('Z', instruction, sizea);
					else
						fp.z = current_units.z;
				}
				else//相对坐标模式,目标位置是指令中的参数加上当前坐标(以当前当坐标为原点的坐标)
				{
					fp.x = search_string('X', instruction, sizea) + current_units.x;
					fp.y = search_string('Y', instruction, sizea) + current_units.y;
					fp.z = search_string('Z', instruction, sizea) + current_units.z;
				}
        
			break;
		}

		//do something!
		switch (code)//这里还是G代码
		{
			//Rapid Positioning
			//Linear Interpolation
			//these are basically the same thing.
			case 0://G0和G1一样
			case 1:
				//set our target.
				set_target(fp.x, fp.y, fp.z);//设定目标坐标,准备移动
       if(search_string('F', instruction, sizea)>0)//如果F后的值不为0
       {
          feedrate = search_string('F', instruction, sizea);//提取指令中的速度值
          pre_feed=feedrate;//前一次的速度
       }
       else//如果没有给定速度
          feedrate =pre_feed;//使用前一次的速度
		  //原固件每执行一条指令feedrate都会被清0,导致每一条G代码都得给定F,这里做了修改
				//do we have a set speed?
				if (has_command('G', instruction, sizea))//这段代码感觉有点多余
				{
					//adjust if we have a specific feedrate.
					if (code == 1)
					{
						//how fast do we move?
						//feedrate = search_string('F', instruction, sizea);
						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);//移动
			break;
			
			//Clockwise arc
			case 2://顺圆插补
			//Counterclockwise arc
			case 3://逆圆插补
				FloatPoint cent;

				// Centre coordinates are always relative
				cent.x = search_string('I', instruction, sizea) + current_units.x;
				cent.y = search_string('J', instruction, sizea) + current_units.y;//圆心的绝对坐标,ij参数是圆心相对于起点的坐标
				float angleA, angleB, angle, radius, le, 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);
				}
				//这里计算的以圆心为起点,目标坐标和当前坐标为终点的矢量与x轴的夹角(从x轴开始逆时针旋转)
				//画个图可以帮助理解
				// 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);//半径
				le = radius * angle;//弧长
				int steps, s, stepa;
				steps = (int) ceil(le / curve_section);//对弧长进行分片,并取最大整数

				FloatPoint newPoint;
				for (s = 1; s <= steps; s++) {
					stepa = (code == 3) ? s : steps - s; // G03:stepa=s,G02:stepa=steps-s(判断的一种特殊写法)

					//这里以顺圆为例,可以画图理解
					//每个片段的终点
					newPoint.x = cent.x + radius * cos(angleA + angle * ((float) stepa / steps));
					//对于顺圆, angle * ((float) stepa / steps)计算的是剩余片段弧长对应的圆心角
					//angleA + angle * ((float) stepa / steps)是以圆心为起点,该片段终点为终点的矢量与x轴的夹角
					//到这里这个矢量的大小(等于半径)、方向、起点都已知,通过计算就可以得到片段终点
					newPoint.y = cent.y + radius * sin(angleA + angle * ((float) stepa / steps));
					set_target(newPoint.x, newPoint.y, fp.z);

					// 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);//移动
				}
	
			break;

			//Dwell
			case 4://G4
				delay((int)search_string('P', instruction, sizea));//延迟
			break;

			//Inches for Units
			case 20://G20
				x_units = X_STEPS_PER_INCH;
				y_units = Y_STEPS_PER_INCH;
				z_units = Z_STEPS_PER_INCH;
				curve_section = CURVE_SECTION_INCHES;//以英寸为单位
				
				calculate_deltas();
			break;

			//mm for Units
			case 21://G21以mm为单位
				x_units = X_STEPS_PER_MM;
				y_units = Y_STEPS_PER_MM;
				z_units = Z_STEPS_PER_MM;
				curve_section = CURVE_SECTION_MM;
				
				calculate_deltas();
			break;

			//go home.
			case 28://G28回原点
				set_target(0.0, 0.0, 0.0);
				dda_move(getMaxSpeed());
			break;

			//go home via an intermediate point.
			case 30://G30设定并移动到目标位置再回0
				fp.x = search_string('X', instruction, sizea);
				fp.y = search_string('Y', instruction, sizea);
				fp.z = search_string('Z', instruction, sizea);

				//set our target.
				if(abs_mode)//绝对坐标模式
				{
					if (!has_command('X', instruction, sizea))
						fp.x = current_units.x;//没有给定目标坐标就保持当前坐标
					if (!has_command('Y', instruction, sizea))
						fp.y = current_units.y;
					if (!has_command('Z', instruction, sizea))
						fp.z = current_units.z;
						
					set_target(fp.x, fp.y, fp.z);
				}
				else
					set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z);//相对坐标模式
				
				//go there.
				dda_move(getMaxSpeed());

				//go home.
				set_target(0.0, 0.0, 0.0);//回0
				dda_move(getMaxSpeed());
			break;

			//Absolute Positioning
			case 90://G90绝对坐标模式
				abs_mode = true;
			break;

			//Incremental Positioning
			case 91://G91相对坐标模式
				abs_mode = false;
			break;

			//Set as home
			case 92:
				set_position(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:
				Serial.print("huh? G");
				Serial.println(code,DEC);
		}
	}
	
	//find us an m code.
	if (has_command('M', instruction, sizea))//M代码
	{
		code = search_string('M', instruction, sizea);
		switch (code)
		{
			//TODO: this is a bug because search_string returns 0.  gotta fix that.
			case 0:
				true;
			break;
	/*
			case 0:
				//todo: stop program
			break;

			case 1:
				//todo: optional stop
			break;

			case 2:
				//todo: program end
			break;
	*/		
			//set max extruder speed, 0-255 PWM
			case 3:
				a.write(150);
				delay(300);
			break;
			case 5:
				a.write(50);
				delay(300);
			break;//这个是我加入的,利用m03和m05控制舵机
			//以下都是与extruder(挤出机)有关的M代码,不作解释
			case 100:
				extruder_speed = (int)(search_string('P', instruction, sizea));
			break;

			//turn extruder on, forward
			case 101:
				extruder_set_direction(1);
				extruder_set_speed(extruder_speed);
			break;

			//turn extruder on, reverse
			case 102:
				extruder_set_direction(0);
				extruder_set_speed(extruder_speed);
			break;

			//turn extruder off
			case 103:
				extruder_set_speed(0);
			break;

			//custom code for temperature control
			case 104:
				extruder_set_temperature((int)search_string('P', instruction, sizea));

				//warmup if we're too cold.
				while (extruder_get_temperature() < extruder_target_celsius)
				{
					extruder_manage_temperature();
					Serial.print("T:");
					Serial.println(extruder_get_temperature());
					delay(1000);	
				}
				
			break;

			//custom code for temperature reading
			case 105:
				Serial.print("T:");
				Serial.println(extruder_get_temperature());
			break;
			
			//turn fan on
			case 106:
				extruder_set_cooler(255);
			break;

			//turn fan off
			case 107:
				extruder_set_cooler(0);
			break;
			
			default:
				Serial.print("Huh? M");
				Serial.println(code);
		}		
	}
	
	//tell our host we're done.
	Serial.println("ok");
//	Serial.println(line, DEC);
}

//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[10] = "";//要提取的数值(先以字符数组储存)

	for (byte i=0; i<string_size; i++)//遍历指令数组
	{
		if (instruction[i] == key)//如果存在要寻找的字符
		{
			i++;//不储存搜索的这个字符      
			int k = 0;
			while (i < string_size && k < 10)//在指令长度范围内且提取的数值不超过10时
			{
				if (instruction[i] == 0 || instruction[i] == ' ')//如果没有元素被接收(这里=0并不是数组元素=0,数组是char型的)或者接收到空格时,终止循环
					break;

				temp[k] = instruction[i];//把元素存入temp数组
				i++;
				k++;//累加,进行下一个
			}
		return strtod(temp, NULL);//把temp[]转化成浮点型的数,并返回
		}
	}
	
	return 0;//默认返回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[i] == key)//存在要寻找的字符
			return true;//返回真
	}
	
	return false;//默认返回false
}

stepper_contorl

//这个文件包含步进电机控制的一些函数
//首先需要知道步进电机的驱动方式:步进电机上有3个引脚,step、dir、enable
//step每接收一个脉冲运动一步,脉冲与脉冲之间的延迟则决定了电机的转速
//控制dir的高低电平实现电机正反转
//控制enable的高低电平打开或者关闭步进电机
//init our variables
long max_delta;
long x_counter;
long y_counter;
long z_counter;
bool x_can_step;
bool y_can_step;
bool z_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;
	target_units.x = 0.0;
	target_units.y = 0.0;
	target_units.z = 0.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);
	//初始化针脚
	//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);//启动步进电机
	

	max_delta = max(delta_steps.x, delta_steps.y);
	max_delta = max(delta_steps.z, max_delta);
	

	//init stuff.
	long x_counter = -max_delta/2;
	long y_counter = -max_delta/2;
	long z_counter = -max_delta/2;
	
	//our step flags
	bool x_can_step = 0;
	bool y_can_step = 0;
	bool z_can_step = 0;
	
	//if (micro_delay >= 16383)
		//milli_delay = micro_delay / 1000;
	//else
		//milli_delay = 0;//这里注释掉了,不知道为什么要换算成毫秒

	//do our DDA line!
	do
	{
		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);
		
		//以下是bresenham算法
		//http://blog.csdn.net/caogos/article/details/52354686

		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++;//正方向移动,累加当前步数坐标
				else
					current_steps.x--;
			}
		}

		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++;
				else
					current_steps.y--;
			}
		}
		
		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++;
				else
					current_steps.z--;
			}
		}
		
		//extruder_manage_temperature();
				
		//
		//if (milli_delay > 0)
			//delay(milli_delay);			
		//else
			delayMicroseconds(micro_delay);//脉冲之间的延迟
     
	}
	while (x_can_step || y_can_step || z_can_step);//当允许步进电机运动时(没有触发限位且未到达目标位置)
	
	//set our points to be the same
	current_units.x = target_units.x;
	current_units.y = target_units.y;
	current_units.z = target_units.z;//移动结束后,当前坐标就是目标坐标
	calculate_deltas();//计算相关参数,为下一次移动做准备
}

bool can_step(byte min_pin, byte max_pin, long current, long target, byte directiona)//允许步进电机运动,返回true
{
	//stop us if we're on target
	if (target == current)//当到达目标位置时,返回false
		return false;
	//stop us if we're at home and still going 
	else if (read_switch(min_pin) && !directiona)//当最小限位触发,且方向为负时返回false
		return false;
	//stop us if we're at max and still going
	else if (read_switch(max_pin) && directiona)//当最大限位触发,且方向为正时返回false
		return false;

	//default to being able to step
	return true;//默认为真
}

void do_step(byte step_pin)//步进电机走一步
{
	digitalWrite(step_pin, HIGH);
	delayMicroseconds(5);
	digitalWrite(step_pin, LOW);//发一个脉冲
}

bool read_switch(byte pin)//读取限位开关状态,触发时返回true
{
	//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;//走1mm需要的脉冲数*坐标
}

void set_target(float x, float y, float z)//设定目标坐标
{
	target_units.x = x;
	target_units.y = y;
	target_units.z = z;
	
	calculate_deltas();
}

void set_position(float x, float y, float z)//设定当前坐标
{
	current_units.x = x;
	current_units.y = y;
	current_units.z = z;
	
	calculate_deltas();
}

void calculate_deltas()//计算运动相关参数
{
	//figure our deltas.
	delta_units.x = abs(target_units.x - current_units.x);//每个轴的坐标差值,|当前坐标-目标坐标|
	delta_units.y = abs(target_units.y - current_units.y);
	delta_units.z = abs(target_units.z - current_units.z);
				
	//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);

	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);

	delta_steps.x = abs(target_steps.x - current_steps.x);//到达目标位置还需要走的步数
	delta_steps.y = abs(target_steps.y - current_steps.y);
	delta_steps.z = abs(target_steps.z - current_steps.z);
	
	//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);

	//set our direction pins as well
	digitalWrite(X_DIR_PIN, x_direction);
	digitalWrite(Y_DIR_PIN, y_direction);
	digitalWrite(Z_DIR_PIN, z_direction);
}


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);//计算距离
	long master_steps = 0;
	
	//选出需要移动最多步数的那一个电机
	//因为实现3轴联动的时候,运动的时间取决于需要移动最多距离的那一轴
	if (delta_steps.x > delta_steps.y)
	{
		if (delta_steps.z > delta_steps.x)
			master_steps = delta_steps.z;
		else
			master_steps = delta_steps.x;
	}
	else
	{
		if (delta_steps.z > delta_steps.y)
			master_steps = delta_steps.z;
		else
			master_steps = delta_steps.y;
	}

	//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*60000000) / feedrate) / master_steps;	//距离/速度=时间,时间/最大步数=脉冲延迟,*60000000是换算成微秒
}

long getMaxSpeed()//计算以最大速度运行需要的脉冲延迟
{
	if (delta_steps.z > 0)//如果z轴需要运动,取Z轴的最大速度(Z轴速度一般不能太快)
		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);
}

 

共有 人打赏支持
粉丝 2
博文 4
码字总数 10803
×
YANG_G
如果觉得我的文章对您有用,请随意打赏。您的支持将鼓励我继续创作!
* 金额(元)
¥1 ¥5 ¥10 ¥20 其他金额
打赏人
留言
* 支付类型
微信扫码支付
打赏金额:
已支付成功
打赏金额: