運動控制入門篇GCode_Interpreter

xmdesign · 發表于 2014-5-10 09:39:53

本帖最后由 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:48:37

本帖最后由 xmdesign 于 2014-5-10 09:52 編輯

GCode_Interpreter是比較容易懂的，木那些寄存器等蝦米開始不容易懂的東東
貼代碼先
直接Maple的，某寶許多超便宜哈
暈，我怎么上不了RAR?
想玩的留下 e妹吧
第一個妹麻煩傳第二個妹哈
我平常雜事多
誰放網盤上再，麻煩開放下

xmdesign · 發表于 2014-5-10 09:52:53

// 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[COMMAND_SIZE];
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[serial_count] = 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();

}

xmdesign · 發表于 2014-5-10 09:53:25

// 定義機器參數
#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_FEEDRATE 1500.0
#define FAST_U_FEEDRATE 1500.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

xmdesign · 發表于 2014-5-10 09:54:26

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

luxiang821 · 發表于 2014-5-10 09:54:28

樓主推薦的網址不錯
請問樓主是玩什么的，用樂高玩具嗎？

xmdesign · 發表于 2014-5-10 09:55:02

// 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[i] = 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[0] == '/')
  {
// 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[10] = "       ";
  for (byte i=0; i<string_size; i++)
  {
if (instruction[i] == key)
{
   i++;
   int k = 0;
   while (i < string_size && k < 10)
   {
      if (instruction[i] == 0 || instruction[i] == ' ')
      break;

      temp[k] = instruction[i];
      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[i] == key){

   return true;
}
  }

  return false;
}

xmdesign · 發表于 2014-5-10 09:55:37

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

xmdesign · 發表于 2014-5-10 09:58:04

基本原代碼的大概是這樣，我是斷斷續續搗鼓的，玩到最后版的，后面我會找出來先測試下過，再貼上：）

duanyz · 發表于 2014-5-10 10:48:17

請樓主發一份給我，感謝！178354773@qq.com

		自動登錄	找回密碼
密碼			注冊會員

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

運動控制入門篇GCode_Interpreter