firmware

#define LED_PIN            13

#define X_STEP_PIN         54
#define X_DIR_PIN          55
#define X_ENABLE_PIN       38

#define Y_STEP_PIN         60
#define Y_DIR_PIN          61
#define Y_ENABLE_PIN       56

#define Z_STEP_PIN         46
#define Z_DIR_PIN          48
#define Z_ENABLE_PIN       62

#define INPUT_SIZE 30

// todo: command to get state of motors (enabled or not)
// todo: command to get microstep value

enum {
  CMD_NOP,
  // Start / Stop motor
  CMD_MOTOR_X_START, // SX
  CMD_MOTOR_Y_START, // SY
  CMD_MOTOR_X_STOP,  // EX
  CMD_MOTOR_Y_STOP,  // EY
  CMD_MOVE_RELATIVE_X, // MX relative_pos
  CMD_MOVE_RELATIVE_Y, // MY relative_pos
  CMD_MOVE_TO_X, // PX pos
  CMD_MOVE_TO_Y, // PY pos
  CMD_GET_POS, // POS
};

//const int stepsPerRevolution = 200;

int microsteps = 2;
int pulse_width = 10;

int max_speed = 50;
int default_speed = 10;
int motor_speed = default_speed;

// 5: se coince, perte de pas
int millis_between_steps = 100.0 / motor_speed;

char input[INPUT_SIZE + 1];

// assuming the stage is centered
int xpos = 0;
int ypos = 0;

const int X_MIN =-400;
const int X_MAX = 400;
const int Y_MIN =-400;
const int Y_MAX = 400;

bool motor_x_is_on = false;
bool motor_y_is_on = false;

// MX, MY: pos
int arg0 = 0;
// MX, MY: speed
int arg1 = 0;

void enableMotorX(bool enabled) {
  if(motor_x_is_on != enabled) {
    digitalWrite(X_ENABLE_PIN, enabled? LOW: HIGH);
    motor_x_is_on = enabled;

    if(enabled) {
      Serial.println("Start Motor X");
    } else {
      Serial.println("Stop Motor X");
    }
  }
}
void enableMotorY(bool enabled) {
  if(motor_y_is_on != enabled) {
    digitalWrite(Y_ENABLE_PIN, enabled? LOW: HIGH);
    motor_y_is_on = enabled;

    if(enabled) {
      Serial.println("Start Motor Y");
    } else {
      Serial.println("Stop Motor Y");
    }
  }
}

void setup()
{
  // 9600 bits per seconds
  Serial.begin(9600);

  pinMode(X_STEP_PIN, OUTPUT);
  pinMode(X_DIR_PIN, OUTPUT);
  pinMode(X_ENABLE_PIN, OUTPUT);

  pinMode(Y_STEP_PIN, OUTPUT);
  pinMode(Y_DIR_PIN, OUTPUT);
  pinMode(Y_ENABLE_PIN, OUTPUT);

  // disable motors
  enableMotorX(false);
  enableMotorY(false);
}


byte parseCommand(const char* input)
{
  byte command = CMD_NOP;

  // Move relative
  if(input[0] == 'M' && input[2] == ' ') {
    if(input[1] == 'X') {
      command = CMD_MOVE_RELATIVE_X;
    }
    else if(input[1] == 'Y') {
      command = CMD_MOVE_RELATIVE_Y;
    }
  }

  // Move absolute
  else if(input[0] == 'P' && input[2] == ' ') {
    if(input[1] == 'X') {
      command = CMD_MOVE_TO_X;
    }
    else if(input[1] == 'Y') {
      command = CMD_MOVE_TO_Y;
    }
  }

  // Start motor
  else if(input[0] == 'S') {
    if(input[1] == 'X') {
      command = CMD_MOTOR_X_START;
    }
    else if(input[1] == 'Y') {
      command = CMD_MOTOR_Y_START;
    }
  }

  // Stop motor
  else if(input[0] == 'E') {
    if(input[1] == 'X') {
      command = CMD_MOTOR_X_STOP;
    }
    else if(input[1] == 'Y') {
      command = CMD_MOTOR_Y_STOP;
    }
  }

  else if(input[0] == 'P' && input[1] == 'O' && input[2] == 'S') {
    command = CMD_GET_POS;
  }

  return command;
}

bool execCommandWithoutArguments(byte command)
{
  if(command == CMD_MOTOR_X_START) {
    enableMotorX(true);
  }
  else if(command == CMD_MOTOR_Y_START) {
    enableMotorY(true);
  }
  else if(command == CMD_MOTOR_X_STOP) {
    enableMotorX(false);
  }
  else if(command == CMD_MOTOR_Y_STOP) {
    enableMotorY(false);
  }
  else if(command == CMD_GET_POS) {
    char str[10];
    snprintf(str, 9, "%d", xpos);
    Serial.print(str);
    Serial.print(",");
    snprintf(str, 9, "%d", ypos);
    Serial.println(str);
  }
  else {
    return false;
  }
  return true;
}

void parseCommandArguments()
{
  char* token = strtok(input, " ");

  int index = 0;
  while(token != NULL)
  {
    if(index == 1) {
      arg0 = atoi(token);
    }
    else if(index == 2) {
      arg1 = atoi(token);
    }

    //Serial.println(token);
    token = strtok(NULL, " ");
    ++index;
  }
}

void setMotorDirFromArg0_relative(int dir_pin)
{
  if(arg0 < 0) {
    arg0 = -arg0;
    digitalWrite(dir_pin, LOW);
  } else {
    digitalWrite(dir_pin, HIGH);
  }
}

void setMotorDir(int dir_pin, bool reverse)
{
  if(reverse) {
    digitalWrite(dir_pin, LOW);
  } else {
    digitalWrite(dir_pin, HIGH);
  }
}

void setMotorSpeedFromArg1()
{
  int motor_speed = arg1;
  if(motor_speed <= 0) {
    motor_speed = default_speed;
  }
  if(motor_speed > max_speed) {
    motor_speed = max_speed;
  }
  millis_between_steps = 100.0 / motor_speed;
}

void rotate(int step_pin, int distance)
{
  for(int x=0; x<distance; x++)
  {
    digitalWrite(step_pin, HIGH);
    delayMicroseconds(pulse_width);
    digitalWrite(step_pin, LOW);
    delay(millis_between_steps);
  }
}

void execCommandWithArguments(byte command)
{
  if(command == CMD_MOVE_RELATIVE_X) {
    // check bounds
    if(arg0 > 0) {
      if(xpos == X_MAX) {
        return;
      }
      if(xpos + arg0 > X_MAX) {
        arg0 = X_MAX - xpos;
      }
    }
    else if(arg0 < 0) {
      if(xpos == X_MIN) {
        return;
      }
      if(xpos + arg0 < X_MIN) {
        arg0 = X_MIN - xpos;
      }
    }

    if(arg0 == 0) {
      return;
    }

    enableMotorX(true);
    xpos += arg0;
    setMotorDirFromArg0_relative(X_DIR_PIN);
    setMotorSpeedFromArg1();

    int distance = arg0 * microsteps;
    rotate(X_STEP_PIN, distance);
  }

  else if(command == CMD_MOVE_RELATIVE_Y) {
    // check bounds
    if(arg0 > 0) {
      if(ypos == Y_MAX) {
        return;
      }
      if(ypos + arg0 > Y_MAX) {
        arg0 = Y_MAX - ypos;
      }
    }
    else if(arg0 < 0) {
      if(ypos == Y_MIN) {
        return;
      }
      if(ypos + arg0 < Y_MIN) {
        arg0 = Y_MIN - ypos;
      }
    }

    if(arg0 == 0) {
      return;
    }

    enableMotorY(true);
    ypos += arg0;
    setMotorDirFromArg0_relative(Y_DIR_PIN);
    setMotorSpeedFromArg1();

    int distance = arg0 * microsteps;
    rotate(Y_STEP_PIN, distance);
  }
  else if(command == CMD_MOVE_TO_X)
  {
    arg0 = constrain(arg0, X_MIN, X_MAX);
    if(xpos == arg0) {
      return;
    }

    enableMotorX(true);
    setMotorDir(X_DIR_PIN, arg0 < xpos);
    setMotorSpeedFromArg1();

    int distance;
    if(arg0 < xpos) {
      distance = (xpos - arg0) * microsteps;
    } else {
      distance = (arg0 - xpos) * microsteps;
    }
    xpos = arg0;

    rotate(X_STEP_PIN, distance);
  }
  else if(command == CMD_MOVE_TO_Y)
  {
    arg0 = constrain(arg0, Y_MIN, Y_MAX);
    if(ypos == arg0) {
      return;
    }

    enableMotorY(true);
    setMotorDir(Y_DIR_PIN, arg0 < ypos);
    setMotorSpeedFromArg1();

    int distance;
    if(arg0 < ypos) {
      distance = (ypos - arg0) * microsteps;
    } else {
      distance = (arg0 - ypos) * microsteps;
    }
    ypos = arg0;

    rotate(Y_STEP_PIN, distance);
  }
}

void loop()
{
  if(Serial.available()) {
    byte size = Serial.readBytes(input, INPUT_SIZE);
    input[size] = 0;

    byte command = parseCommand(input);
    if(!execCommandWithoutArguments(command))
    {
      arg0 = 0;
      arg1 = 0;
      parseCommandArguments();
      execCommandWithArguments(command);
    }
  }
}