Tinkers Projects

Code for Project

#include "config.h"
#include <SPI.h>
#include <SD.h>
//http://marlinfw.org/meta/gcode/
#include <Arduino.h>
#include <gcode.h>
#include <AccelStepper.h>
#include <Plex.h>


#define XMultiplier 80
#define YMultiplier 80
#define CircleRatio 1

#define Speed 1000 //200
#define Acceleration 50000000.0

#define XUpperLimitSelect 2
#define XLowerLimitSelect 3
#define YUpperLimitSelect 4
#define YLowerLimitSelect 5
#define scrollButton 6
#define selectButton 7

#define ASelect 1
#define BSelect 7

//#define machineInputPin A7
//#define UserInputPin A4

#define StepPin A2
#define DirPin A5
#define enablePin A4
#define PlasmaPin 3
#define SDchipSelectPin 10


const float pi = 3.14159265359;


double X;
double Y;
int encoder0Pos = 0;
int encoder0PinALast = LOW;

unsigned long currentPosInFile = 0;
bool Running = false;
long inPos = 0;

void homeMachine();
void PlasmaOn();
void PlasmaOff();
void Enabled();
void Disabled();
commandscallback commands[6] = {{"G28",homeMachine},
                                {"M3",PlasmaOn},
                                {"M4",PlasmaOn},
                                {"M5",PlasmaOff},
                                {"M17",Enabled},
                                {"M18",Disabled}};


gcode Commands(6,commands);
AccelStepper stepperA;
AccelStepper stepperB;
Plex PLEX;

File dir;
File Files;

int FileNumber = 0;
bool displayed = false;

void setup()
{
  pinMode(enablePin, OUTPUT);
  pinMode(PlasmaPin, OUTPUT);
  digitalWrite(enablePin,HIGH);
  digitalWrite(PlasmaPin,LOW);

  Commands.begin("OK");
  
  stepperA = AccelStepper(1,StepPin,DirPin);
  stepperA.setMaxSpeed(Speed);
  stepperA.setCurrentPosition(0);
  stepperA.setAcceleration(Acceleration);
  
  stepperB = AccelStepper(1,StepPin,DirPin);
  stepperB.setMaxSpeed(Speed);
  stepperB.setCurrentPosition(0);
  stepperB.setAcceleration(Acceleration);
  
  PLEX.lcd.setCursor(0, 0);
  PLEX.lcd.print("Start");
}

void loop() 
{
  if (!SD.begin(SDchipSelectPin)) 
  {
    PLEX.lcd.setCursor(0,0);
    PLEX.lcd.print("can not read SD!");
    PLEX.lcd.setCursor(0,1);
    PLEX.lcd.print("                ");
    displayed = false;
    FileNumber=0;
  }
  else
  {
    if (PLEX.digitalReadC(scrollButton) == HIGH) 
    {
      Serial.println("scrollButton");
      while(PLEX.digitalReadC(scrollButton) == HIGH);
      FileNumber++;
      displayed = false;
    }

    if(displayed)
    {
      if(FileNumber<0)
        FileNumber=0;
      Files = getFile(FileNumber); 
      if(Files) 
      {
        PrintFileName(Files);
        displayed = true; 
      }
      else if(FileNumber == 0) 
      {
        PLEX.lcd.clear();
        PLEX.lcd.print("can not read SD!");
        displayed = true; 
      }
      else
      {
        FileNumber--;
      }
    }

    if (PLEX.digitalReadC(selectButton) == HIGH) 
    {
      Serial.println("selectButton");
      File dataFile = getFile(FileNumber);
      if(dataFile) 
      {
        Enabled();
        while (dataFile.available()) 
        {
          char SDinput = dataFile.read();
          Serial.print(SDinput);
          if(Commands.available(SDinput))
          {
            if(Commands.GetValue('G') == 0 || Commands.GetValue('G') == 1)
            {      
              Serial.println("gotoLocation");
              gotoLocation();
            }
            else if(Commands.GetValue('G') == 2)      
              arc(1);
            else if(Commands.GetValue('G') == 3)      
              arc(0);
            else if(Commands.GetValue('G') == 4)      
            {
              if(Commands.availableValue('S'))
                delay(Commands.GetValue('S')*1000);
              else if(Commands.availableValue('P'))
                delay(Commands.GetValue('P'));
            }
            Commands.clearBuffer();
          }
        }
        Disabled();
      }
      dataFile.close();
      PLEX.lcd.clear();
      PLEX.lcd.print("    Finished    ");
      displayed = false;
      while(PLEX.digitalReadC(selectButton) == LOW);
      while(PLEX.digitalReadC(selectButton) == HIGH);
    }
  }
}


File getFile(int number) 
{
  int count = 0;
  dir = SD.open("/");
  while (true) 
  {
    File entry =  dir.openNextFile();
    if (!entry.available())
    {
      return entry;
      break;
    }
    if (!entry.isDirectory()) 
    {
      if(number == count)return entry;
      count++;
    }
    entry.close();
  }
  return;
}



void PrintFileName(File entry) 
{
    PLEX.lcd.clear();
    PLEX.lcd.setCursor(0,0);
    PLEX.lcd.print(entry.name());
    PLEX.lcd.setCursor(0,1);
    PLEX.lcd.print(entry.size(), DEC);
}




/****************************** MACHINE CONTROL ******************************/

void PlasmaOn()
{
  digitalWrite(PlasmaPin,HIGH);
  PLEX.lcd.clear();
  PLEX.lcd.print("Plasma on");
}

void PlasmaOff()
{
  digitalWrite(PlasmaPin,LOW);
  PLEX.lcd.clear();
  PLEX.lcd.print("Plasma off");
}

void Enabled()
{
  digitalWrite(enablePin,LOW);
  PLEX.lcd.setCursor(0, 0);
  PLEX.lcd.print("Enabled");
}

void Disabled()
{
  digitalWrite(enablePin,HIGH);
  PLEX.lcd.setCursor(0, 0);
  PLEX.lcd.print("Disabled");
}











/****************************** MOTOR CONTROL ******************************/

void gotoLocation()
{
  if(Commands.availableValue('X'))
    X = Commands.GetValue('X');

  if(Commands.availableValue('Y'))
    Y = Commands.GetValue('Y');

  setMotorLocation(X,Y);
  runStepperMotors();
}

void arc(boolean clockwise)
{
  /*
    http://marlinfw.org/docs/gcode/G002-G003.html

    ****** I,J Form ******
      - I specifies an X offset. J specifies a Y offset.
      - At least one of the I J parameters is required.
      - X and Y can be omitted to do a complete circle.
      - The given X Y is not error-checked. The arc ends based on the angle of the destination.
      - Mixing I or J with R will throw an error.
    ****** R Form ******
      - R specifies the radius. X or Y is required.
      - Omitting both X and Y will throw an error.
      - X or Y must differ from the current XY position.
      - Mixing R with I or J will throw an error.
  */
/* 
  double startX = X;
  double startY = Y;
  double finishedX = X;
  double finishedY = Y;
  double XCenter = X;
  double YCenter = Y;
  double radius = 0;

  //I,J Form
  if((Commands.availableValue('I') || Commands.availableValue('J')) && !Commands.availableValue('R'))
  {
    double J = 0;
    double I = 0;

    if(Commands.availableValue('I')) 
      I = Commands.GetValue('I');

    if(Commands.availableValue('J')) 
      J = Commands.GetValue('J');

    if(Commands.availableValue('X'))
    {
      finishedX = Commands.GetValue('X');
    }
    if(Commands.availableValue('Y'))
    {
      finishedY = Commands.GetValue('Y');
    }

    XCenter = startX + I;
    YCenter = startY + J;
    radius = sqrt(J*J+I*I);
  }
  // R Form
  else if (!Commands.availableValue('I') && !Commands.availableValue('J') && Commands.availableValue('R') && (Commands.availableValue('X') || Commands.availableValue('Y')))
  {
    if(Commands.availableValue('R')) 
      radius = Commands.GetValue('R');

    if(Commands.availableValue('X'))
    {
      finishedX = Commands.GetValue('X');
    }
    if(Commands.availableValue('Y'))
    {
      finishedY = Commands.GetValue('Y');
    }
/*
    double q = sqrt((finishedX-startX)*(finishedX-startX) + (finishedY-startY)*(finishedY-startY));

    double x1 = x3 + sqrt(r^2-(q/2)*(q/2))*(startY-finishedY)/q
    double y1 = y3 + sqrt(r^2-(q/2)*(q/2))*(finishedX-startX)/q 

    double x2 = x3 - sqrt(r^2-(q/2)*(q/2))*(startY-finishedY)/q
    double y2 = y3 - sqrt(r^2-(q/2)*(q/2))*(finishedX-startX)/q

    XCenter = x1;
    YCenter = x2;* /
  }

  float startAngle = atan2((startY-YCenter), (startX-XCenter));
  float finishedAngle = atan2((finishedY-YCenter), (finishedX-XCenter));
  
  float Curcunfrence = atan2((finishedY-YCenter), (finishedX-XCenter));

  double AngleBetween = startAngle - finishedAngle;
  if(startAngle == finishedAngle)
    AngleBetween = 2*pi;
  
 /* if(clockwise)
    AngleBetween = 2*pi - AngleBetween; 
* /

  double NewfinishedAngle = startAngle - finishedAngle;

/*
  if(clockwise)
  {
    if(finishedAngle > startAngle)
      finishedAngle = finishedAngle-2*pi;

    for(double i = startAngle; i<finishedAngle)
    {
      setMotorLocation(X,Y);
      runStepperMotors();
    }
  }
  else
  {
    if(finishedAngle < startAngle)
      finishedAngle = finishedAngle+2*pi;

    for(double i = startAngle; i)
    {
      setMotorLocation(X,Y);
      runStepperMotors();
    }
  }

  setMotorLocation(finishedX,finishedY);
  runStepperMotors();

  Serial.println();
  Serial.println(startX);
  Serial.println(startY);
  Serial.println(finishedX);
  Serial.println(finishedY);
  Serial.println(J);
  Serial.println(I);
  
  Serial.println(YCenter);
  Serial.println(radius);
  Serial.println(startAngle);
  Serial.println(finishedAngle);
  Serial.println(AngleBetween);* /
  */
}

void homeMachine()
{
  /*setMotorLocation(-100000,-100000);
  while((PLEX.digitalReadC(XUpperLimitSelect) == LOW && PLEX.digitalReadC(XLowerLimitSelect) == LOW) || (PLEX.digitalReadC(YUpperLimitSelect) == LOW && PLEX.digitalReadC(YLowerLimitSelect) == LOW))
  {
    PLEX.selectD(BSelect);
    stepperA.run();
    PLEX.selectD(ASelect);
    stepperB.run();
  }
  X = 0;
  Y = 0;
  stepperA.setCurrentPosition(0);
  stepperB.setCurrentPosition(0);*/
}



void setMotorLocation(double NewX,double NewY)
{
  X = NewX;
  Y = NewY;
  double x = NewX*XMultiplier;
  double y = NewY*YMultiplier;
  
  double XCurrent = 0.5*(stepperA.currentPosition() + stepperB.currentPosition());
  double YCurrent = 0.5*(stepperA.currentPosition() - stepperB.currentPosition());
  double R = sqrt((y-YCurrent)*(y-YCurrent)+(x-XCurrent)*(x-XCurrent));
  double t = R/Speed;

  double a = x+y;
  double b = x-y;
  
  double Speeda = abs(a - stepperA.currentPosition())/t;
  double Speedb = abs(b - stepperB.currentPosition())/t;
   
  stepperA.setMaxSpeed(Speeda);
  stepperB.setMaxSpeed(Speedb);
  stepperA.setSpeed(Speeda);
  stepperB.setSpeed(Speedb);
  stepperA.moveTo(a);
  stepperB.moveTo(b);
  
  PLEX.lcd.setCursor(0, 1);
  PLEX.lcd.print(NewX);
  PLEX.lcd.print(",");
  PLEX.lcd.print(NewY);
  PLEX.lcd.print("         ");
}

double currentX()
{
  return (int)(0.5*(stepperA.currentPosition() + stepperB.currentPosition()));
}

double currentY()
{
  return (int)(0.5*(stepperA.currentPosition() - stepperB.currentPosition()));
}

void runStepperMotors()
{
  //if(testLimit(XUpperLimitSelect) == HIGH && currentX()<X){ setMotorLocation(currentX(),Y);}
  //if(testLimit(XLowerLimitSelect) == HIGH && currentX()>X){ setMotorLocation(currentX(),Y);}
  
  //if(testLimit(YUpperLimitSelect) == HIGH && currentY()<Y){ setMotorLocation(X,currentY());}
  //if(testLimit(YLowerLimitSelect) == HIGH && currentY()>Y){ setMotorLocation(X,currentY());}
  while(stepperA.distanceToGo() != 0 || stepperB.distanceToGo() != 0)
  {
    PLEX.selectD(BSelect);
    stepperA.run();
    PLEX.selectD(ASelect);
    stepperB.run();
  }
}