Code for my CNC mill

gistfile1.txt

#define X_STEP_PIN         54
#define X_DIR_PIN          55
#define X_ENABLE_PIN       38
#define X_MIN_PIN           3
#define X_MAX_PIN           2

#define Y_STEP_PIN         60
#define Y_DIR_PIN          61
#define Y_ENABLE_PIN       56
#define Y_MIN_PIN          14
#define Y_MAX_PIN          15

#define Z_STEP_PIN         46
#define Z_DIR_PIN          48
#define Z_ENABLE_PIN       62
#define Z_MIN_PIN          18
#define Z_MAX_PIN          19

#define E_STEP_PIN         26
#define E_DIR_PIN          28
#define E_ENABLE_PIN       24

#define Q_STEP_PIN         36
#define Q_DIR_PIN          34
#define Q_ENABLE_PIN       30

#define SDPOWER            -1
#define SDSS               53
#define LED_PIN            13

#define FAN_PIN            9

#define PS_ON_PIN          12
#define KILL_PIN           -1

#define HEATER_0_PIN       10
#define HEATER_1_PIN       8
#define TEMP_0_PIN          13   // ANALOG NUMBERING
#define TEMP_1_PIN          14   // ANALOG NUMBERING

long started=0;
String pending="";

void setup() {
  Serial.begin(115200); // 19200 38400 57600 115200
  pending.reserve(255);
  //while (!Serial) {}
  
  pinMode(FAN_PIN , OUTPUT);
  pinMode(HEATER_0_PIN , OUTPUT);
  pinMode(HEATER_1_PIN , OUTPUT);
  pinMode(LED_PIN  , OUTPUT);
  
  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(E_STEP_PIN  , OUTPUT);
  pinMode(E_DIR_PIN    , OUTPUT);
  pinMode(E_ENABLE_PIN    , OUTPUT);
  
  pinMode(Q_STEP_PIN  , OUTPUT);
  pinMode(Q_DIR_PIN    , OUTPUT);
  pinMode(Q_ENABLE_PIN    , OUTPUT);

  digitalWrite(X_ENABLE_PIN    , LOW);
  digitalWrite(Y_ENABLE_PIN    , LOW);
  digitalWrite(Z_ENABLE_PIN    , LOW);
  digitalWrite(E_ENABLE_PIN    , LOW);
  digitalWrite(Q_ENABLE_PIN    , LOW);
 
  cli();//stop interrupts

  //set timer2 interrupt at 16kHz
  TCCR2A = 0;// set entire TCCR2A register to 0
  TCCR2B = 0;// same for TCCR2B
  TCNT2  = 0;//initialize counter value to 0
  // set compare match register for 8khz increments
  OCR2A = 124;// = (16*10^6) / (16000*8) - 1 (must be <256)
  // turn on CTC mode
  TCCR2A |= (1 << WGM21);
  // Set CS21 bit for 8 prescaler
  TCCR2B |= (1 << CS21);   
  // enable timer compare interrupt
  TIMSK2 |= (1 << OCIE2A);
/*
  //set timer0 interrupt at 2kHz
  TCCR0A = 0;// set entire TCCR0A register to 0
  TCCR0B = 0;// same for TCCR0B
  TCNT0  = 0;//initialize counter value to 0
  // set compare match register for 2khz increments
  OCR0A = 124;// = (16*10^6) / (2000*64) - 1 (must be <256)
  // turn on CTC mode
  TCCR0A |= (1 << WGM01);
  // Set CS01 and CS00 bits for 64 prescaler
  TCCR0B |= (1 << CS01) | (1 << CS00);   
  // enable timer compare interrupt
  TIMSK0 |= (1 << OCIE0A);
*/
  sei();//allow interrupts
}

long time=0;
long posnX=0, posnY=0, posnZ=0;

long dirCount = 0, stepCount = 0;

void emit_steps(long d, int dPin, int sPin) {
  if (abs(d) > 0) {
    digitalWrite(dPin, (d > 0 ? HIGH : LOW));
    dirCount++;
    for (long i=0; i<abs(d); i++) {
      digitalWrite(sPin, HIGH);
      digitalWrite(sPin, LOW);
      stepCount += 2;
    }
  }
}

// Emit pulses in a loop until the stepper motors are at the given coordinates.
void move_to(long x, long y, long z) {
  emit_steps(x-posnX, X_DIR_PIN, X_STEP_PIN);
  emit_steps(posnY-y, Y_DIR_PIN, Y_STEP_PIN);
  emit_steps(z-posnZ, Z_DIR_PIN, Z_STEP_PIN);
  emit_steps(z-posnZ, E_DIR_PIN, E_STEP_PIN);
  posnX = x;
  posnY = y;
  posnZ = z;
}

class instruction {
public:
  virtual bool execute() volatile = 0;
};

// If "next" is NULL then we are waiting for another instruction.
volatile instruction *cur = NULL, *next = NULL;

/// Linear interpolation
double lerp(double x0, double x1, double p) {
  return x0 + (x1 - x0) * p;
}

class move_instruction : public instruction {
  long ox, oy, oz, dur, x, y, z;
  double idur;
  public:
  move_instruction(long, long, long, long, long, long, long);
  virtual bool execute() volatile;
};

move_instruction::move_instruction(long ox, long oy, long oz, long dur, long x, long y, long z) {
  this->ox = ox;
  this->oy = oy;
  this->oz = oz;
  this->dur = dur;
  this->idur = 1.0 / dur;
  this->x = x;
  this->y = y;
  this->z = z;
}

bool move_instruction::execute() volatile {
  if (time >= this->dur) {
    // If end of instruction then move to final destination.
    move_to(this->x, this->y, this->z);
    return true;
  } else
    // Interpolate towards the final point.
    if (this->dur > 0) {
      double p = max(0, min(1, double(time)*double(this->idur)));
      move_to(lerp(this->ox, this->x, p),
              lerp(this->oy, this->y, p),
              lerp(this->oz, this->z, p));
    }
  return false;
}

class home1_instruction : public instruction {
  long step, detect;
  long *posn;
  public:
  home1_instruction(int, int, long *);
  virtual bool execute() volatile;
};

home1_instruction::home1_instruction(int step, int detect, long *posn) {
  this->step = step;
  this->detect = detect;
  this->posn = posn;
}

bool home1_instruction::execute() volatile {
  if (digitalRead(this->detect) == LOW) {
    if (time % 16 == 0) {
      digitalWrite(step, HIGH);
      digitalWrite(step, LOW);
    }
    return false;
  }
  *(this->posn) = 0;
  return true;
}

class home2_instruction : public instruction {
  long step1, step2, detect1, detect2;
  long *posn;
  public:
  home2_instruction(int, int, int, int, long *);
  virtual bool execute() volatile;
};

home2_instruction::home2_instruction(int step1, int step2, int detect1, int detect2, long *posn) {
  this->step1 = step1;
  this->step2 = step2;
  this->detect1 = detect1;
  this->detect2 = detect2;
  this->posn = posn;
}

bool home2_instruction::execute() volatile {
  bool up1 = digitalRead(this->detect1) == LOW;
  bool up2 = digitalRead(this->detect2) == LOW;
  if (time % 4 == 0) {
    if (up1) {
      digitalWrite(step1, HIGH);
      digitalWrite(step1, LOW);
    }
    if (up2) {
      digitalWrite(step2, HIGH);
      digitalWrite(step2, LOW);
    }
  }
  if (up1 || up2) return false;
  *(this->posn) = 0;
  return true;
}

long timerTick = 0;

ISR(TIMER2_COMPA_vect){
  timerTick++;
/*
  if (!cur && next) {
    // If there is no current instruction but there is a waiting instruction then begin executing it.
    time = 0;
    cur = next;
    next = NULL;
  }
  time++;
  // If there is a currently executing instruction then execute it.
  if (cur)
    if (cur->execute()) {
      // If execution has completed then delete the current instruction.
      delete cur;
      cur = NULL;
    }
*/
}

// x range is 18000 = 225mm => 80 steps per mm
// y range is 33000 = 417.5mm => 79 steps per mm
// z range is ? 200*16*100 = 320,000 => 
// 320,000 16x microsteps on z moved 125mm rather than 100mm?!
// 20,000 steps => 100 revs => 100mm

// Holes at:
// x = 10, 210 = 800, 16800
// y = 10, 410 = 800, 32800
// z = z - 6/1.25*200*16

long ox=0, oy=0, oz=0;

// If "next" is NULL and data is available from the serial port then we read the instruction.
// Every instruction read causes a response to be sent. This is used to notify the other end
// to send another instruction.

#define MAX_ARGS 5
double arg[MAX_ARGS];

// Number the instructions and when there is an error, request resend from the last known-good
// instruction.

int freeRam () 
{
  extern int __heap_start, *__brkval; 
  int v; 
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval); 
}

void parseMoveArgs(String &command, int &idx, int &numArgs) {
  int beginIdx = idx + 1;
  idx = command.indexOf(" ", beginIdx);
  while (idx != -1 && numArgs < MAX_ARGS) {
    String substr = command.substring(beginIdx, idx);
    arg[numArgs++] = atof(substr.c_str());
    beginIdx = idx + 1;
    idx = command.indexOf(" ", beginIdx);
  }
  String substr = command.substring(beginIdx);
  arg[numArgs++] = atof(substr.c_str());
}

void move(long dur, long x, long y, long z) {
  move_instruction *instr = new move_instruction(ox, oy, oz, dur, x, y, z);
  // Turn interrupts off
  cli();
  // Inject the new instruction.
  next = instr;
  // Reset the interrupt's time
  time = 0;
  sei();
  // Use the coordinates given as the previous coordinates next time.
  ox = x;
  oy = y;
  oz = z;
}

void execute(String command) {
  if (next)
    Serial.println("ERROR: Command buffer overrun");
  int idx = command.indexOf(" ");
  if (idx == -1) idx = command.length();
  String cmd = command.substring(0, idx);
  
  if (cmd == "ready") {
      Serial.println("ready");
  } else if (cmd == "move") {
    int numArgs=0;
    parseMoveArgs(command, idx, numArgs);
    if (numArgs == 4) {
      move(arg[0], arg[1], arg[2], arg[3]);
    } else {
      Serial.print("ERROR: '");
      Serial.print(command);
      Serial.print("' has ");
      Serial.print(numArgs);
      Serial.println(" arguments");
    }
  } else if (cmd == "moverel") {
    int numArgs=0;
    parseMoveArgs(command, idx, numArgs);
    if (numArgs == 4) {
      move(arg[0], arg[1]+ox, arg[2]+oy, arg[3]+oz);
    } else {
      Serial.print("ERROR: '");
      Serial.print(command);
      Serial.print("' has ");
      Serial.print(numArgs);
      Serial.println(" arguments");
    }
  } else if (cmd == "homex") {
      Serial.println("Homing X axis");
      digitalWrite(X_DIR_PIN, LOW);
      home1_instruction *instr = new home1_instruction(X_STEP_PIN, X_MIN_PIN, &posnX);
      cli();
      next = instr;
      time = 0;
      sei();
      ox = 0;
  } else if (cmd == "homey") {
      Serial.println("Homing Y axis");
      digitalWrite(Y_DIR_PIN, HIGH);
      home1_instruction *instr = new home1_instruction(Y_STEP_PIN, X_MAX_PIN, &posnY);
      cli();
      next = instr;
      time = 0;
      sei();
      oy = 0;
  } else if (cmd == "homez") {
      Serial.println("Homing Z axis");
      digitalWrite(Z_DIR_PIN, LOW);
      digitalWrite(E_DIR_PIN, LOW);
      home2_instruction *instr = new home2_instruction(Z_STEP_PIN, E_STEP_PIN, Y_MAX_PIN, Y_MIN_PIN, &posnZ);
      cli();
      next = instr;
      time = 0;
      sei();
      oz = 0;
  } else if (cmd == "on") {
      Serial.println("Turning spindle motor on");
      digitalWrite(HEATER_0_PIN, HIGH);
      delay(1000);
      Serial.println("ok");
  } else if (cmd == "off") {
      Serial.println("Turning spindle motor off");
      digitalWrite(HEATER_0_PIN, LOW);
      delay(3000);
      Serial.println("ok");
  } else if (cmd == "end") {
      Serial.println("Waiting for all instructions to be completed");
      while (cur || next) {}
      Serial.println("finished");
      Serial.println("ok");
  } else if (cmd == "where") {
      Serial.print("Current location is ");
      Serial.print(ox);
      Serial.print(", ");
      Serial.print(oy);
      Serial.print(", ");
      Serial.println(oz);
      Serial.println("ok");
  } else {
      Serial.print("ERROR: Unknown instruction ");
      Serial.println(command);
  }
  //Serial.println(freeRam());
  //Serial.print(dirCount);
  //Serial.print(" ");
  //Serial.println(stepCount);
}

// X high is right
// Y high is forwards
// Z high is upwards
// X min detector is x min
// X max detector is y min
// Y min detector is z1 min
// Y max detector is z1 min

enum HomingState {
  HomingTowardsFast,
  HomingAway,
  HomingTowardsSlow,
  Homed
};

int foo=0;
bool isRaising=true;
HomingState homingState = HomingTowardsFast;

bool hardLeft() { return digitalRead(X_MIN_PIN) == HIGH; }
bool hardBackwards() { return digitalRead(X_MAX_PIN) == HIGH; }
bool hardDown1() { return digitalRead(Y_MIN_PIN) == HIGH; }
bool hardDown2() { return digitalRead(Y_MAX_PIN) == HIGH; }
void disableMotors() {
  digitalWrite(X_ENABLE_PIN    , HIGH);
  digitalWrite(Y_ENABLE_PIN    , HIGH);
  digitalWrite(Z_ENABLE_PIN    , HIGH);
  digitalWrite(E_ENABLE_PIN    , HIGH);
}

void home(int enable, int dir, int step, int detect) {
  int pause=0;
  switch (homingState) {
    case HomingTowardsFast :
      digitalWrite(X_ENABLE_PIN, LOW);
      digitalWrite(X_DIR_PIN, LOW);
      if (hardLeft()) {
        homingState = Homed;
      }
      else
      {
        digitalWrite(X_STEP_PIN, HIGH);
        digitalWrite(X_STEP_PIN, LOW);
        pause += 3;
      }
      break;
    case HomingAway :
      digitalWrite(X_ENABLE_PIN, LOW);
      digitalWrite(X_DIR_PIN, HIGH);
      if (hardLeft()) {
        digitalWrite(X_STEP_PIN, HIGH);
        digitalWrite(X_STEP_PIN, LOW);
        pause += 10;
      }
      else
      {
        homingState = HomingTowardsSlow;
      }
      break;
    case HomingTowardsSlow :
      digitalWrite(X_ENABLE_PIN, LOW);
      digitalWrite(X_DIR_PIN, LOW);
      if (hardLeft()) {
        homingState = Homed;
      }
      else
      {
        digitalWrite(X_STEP_PIN, HIGH);
        digitalWrite(X_STEP_PIN, LOW);
        pause += 10;
      }
      break;
    case Homed :
      break;
  }
  delay(pause);
}

long tick = 0;
long longestWait = 0;

void loop () {
  for (int n=0; n<16; ++n)
    if (tick < timerTick) {
      tick++;
      if (!cur && !next)
        tick = timerTick;
      if (timerTick - tick > longestWait)
        longestWait = timerTick - tick;
      if (!cur && next) {
        // If there is no current instruction but there is a waiting instruction then begin executing it.
        time = 0;
        cur = next;
        next = NULL;
      }
      time++;
      // If there is a currently executing instruction then execute it.
      if (cur)
        if (cur->execute()) {
          // If execution has completed then delete the current instruction.
          delete cur;
          cur = NULL;
          Serial.println("ok");
        }
    }
}

void serialEvent() {
  while (Serial.available()) {
    // Try to receive a byte.
    int c = Serial.read();
    switch (c) {
      case -1:
          // If nothing was read then ignore.
          if (pending.length() > 0 && millis() - started > 3000) {
            Serial.println("ERROR: timeout while reading command");
            started = 0;
            pending = "";
          }
          break;
      case '*':
          pending = "";
          break;
      case '\r':
      case '\n':
          Serial.print("Longest wait: ");
          Serial.println(longestWait);
          longestWait = 0;
          //Serial.print("Command '");
          //Serial.print(pending);
          //Serial.println("' read");
          // Pending command has been read completely and can be executed.
          execute(pending);
          pending = "";
          break;
      default:
          // Another non-terminal byte of the pending command has been read.
          if (c > 31 && c < 128) {
            pending += char(c);
            started = millis();
          }
          break;
    }
  }
}