labautobox

#include <AccelStepper.h>
#include <Servo.h>
Servo linearservo;  // create servo object to control a servo
Servo solderservo;  // create servo object to control a servo
Servo pnpservo;  // create servo object to control a servo
#include <PID_v1.h>
#include <Wire.h>
#include <SoftwareSerial.h>

#define TEMPCONTROL 5

//Define the aggressive and conservative Tuning Parameters
double aggKp=150, aggKi=0.5, aggKd=0;

#define THERMISTOR_PIN A0
#define motorInterfaceType 1

int directionPin = 4;
int estepsPin = 2;
#define dirPin 4
#define stepPin 2
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
//AccelStepper stepper = AccelStepper(motorInterfaceType, estepsPin, directionPin);
//const float stepsperul = 131.43; //resolution of 1ml disposable syringes
const float stepsperul = 1; //resolution of 1ml disposable syringes

int linearservopin = 6;
int solderservopin = 11;
int pnpservopin = 13;

double settemp = 0;
double Setpoint, Input, Output;

int solderpin = 10; 
int pnppin = 9; 


int acc = 30;
int turnon5vpin = 3;
//int pcvpin = 6; 
int tempsensor = A0;
//int levelsensor = A1;
int solderval = 255;
int pnpval = 255;
int pcvval = 255;

String command;
//float currpos;
int fillflag = 1;
int levelval = 50;
int heatval = 50;
int htcnt = 0;
int pumpdelayct = 0;
int pumpdelay = 0;
int pumponflag = 0;
int levelstreamon = 0;

long int currpos;
long int pos;

//Specify the links and initial tuning parameters
//PID myPID(&Input, &Output, &Setpoint, consKp, consKi, consKd, DIRECT);
PID myPID(&Input, &Output, &Setpoint, aggKp, aggKi, aggKd, DIRECT);



#define NUMTEMPS 20
short temptable[NUMTEMPS][2] = {
   {1, 841},
   {54, 255},
   {107, 209},
   {160, 184},
   {213, 166},
   {266, 153},
   {319, 142},
   {372, 132},
   {425, 124},
   {478, 116},
   {531, 108},
   {584, 101},
   {637, 93},
   {690, 86},
   {743, 78},
   {796, 70},
   {849, 61},
   {902, 50},
   {955, 34},
   {1008, 3}
};


void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  linearservo.attach(linearservopin);
  solderservo.attach(solderservopin);
  pinMode(directionPin, OUTPUT);
  pinMode(estepsPin, OUTPUT);
  pnpservo.attach(pnpservopin);
  linearservo.write(128);
  analogWrite(turnon5vpin, 0);
  currpos = 0;
  Setpoint = settemp;
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
  stepper.setAcceleration(acc);
  stepper.setMaxSpeed(1000*16);
}

void loop() {

  Input = read_temp();
   
   double gap = abs(Setpoint-Input); //distance away from setpoint
   myPID.SetTunings(aggKp, aggKi, aggKd);
   myPID.Compute();
   analogWrite(TEMPCONTROL,Output);
   int rawvalue = analogRead(THERMISTOR_PIN);
   float celsius = read_temp();
   int fahrenheit = (((celsius * 9) / 5) + 32);


  if (levelstreamon == 1) {
   Serial.println(celsius);
   delay(500);
  }

 if(Serial.available())
 {
    char c = Serial.read();
    if (c== '\n')
    {
      currpos = parseCommand(command, currpos);
      command = "";
    }
    else 
    {
      command +=c;
    }
 }
 delay(30);
}

float parseCommand(String com, int currpos)
{

  if(com.equalsIgnoreCase("solderon")){
    analogWrite(solderpin, solderval);
    delay(100);
  }  
  else if(com.equalsIgnoreCase("solderoff")){
    analogWrite(solderpin, 0);
    delay(100);
  }  
  else if(com.equalsIgnoreCase("pnpon")){
    analogWrite(pnppin, pnpval);
    delay(100);
  }  
  else if(com.equalsIgnoreCase("pnpoff")){
    analogWrite(pnppin, 0);
    delay(100);
  } 
  else if(com.equalsIgnoreCase("info")){
    Serial.println("pnpboard_3servos");
  }
  else if(com.equalsIgnoreCase("turnon5v")){
    analogWrite(turnon5vpin, 255);
  } 
  else if(com.equalsIgnoreCase("turnoff5v")){
    analogWrite(turnon5vpin, 0);
  }

  else if(com.equalsIgnoreCase("levelstreamon")){
   levelstreamon = 1;
  } 
  else if(com.equalsIgnoreCase("levelstreamoff")){
   levelstreamon = 0;
  } 
  else if(com.equalsIgnoreCase("readlevel")){
     int rawvalue = analogRead(THERMISTOR_PIN);
     int celsius = read_temp();
     Serial.println(celsius);
  }
   else if (com.substring(0,3) == "g1e") {
    String part2 = com.substring(com.indexOf("e")+1,com.indexOf("s"));
    String part3 = com.substring(com.indexOf("s")+1,com.indexOf("a"));
    String part4 = com.substring(com.indexOf("a")+1);
    int vpos = part2.toInt();
    pos = vpos * stepsperul; 
    int vfeed = part3.toInt();
    int feed = vfeed * stepsperul;
    //int acc = part4.toInt();
    //int feed = vfeed;
    int acc = feed/3;
    if ((pos-currpos) < 0) {
      feed = feed * -1;
    }
    //Serial.print("direction ");
    //Serial.println(pos-currpos);
    moveacc((pos-currpos), feed, acc);
    currpos = pos;
   }
   else if(com.equalsIgnoreCase("m114")){
   Serial.print("Step Position: ");
   Serial.println(pos);
   }
   else if(com.equalsIgnoreCase("g92e0")){
    pos = 0;
    currpos = 0;
   }
   else if (com.substring(0,12) == "setsolderval") {
    solderval = com.substring(13).toInt();
   }
   else if (com.substring(0,9) == "setpnpval") {
    pnpval = com.substring(10).toInt();
  }
   else if (com.substring(0,9) == "pastetemp") {
    settemp = com.substring(10).toInt();
  }
  
  //else if (com.substring(0,(com.indexOf("linearservopos")-1)) == "linearservopos") {
  else if (com.substring(0,14) == "linearservopos") {
    linearservo.write(com.substring(15).toInt());
  }
  else if (com.substring(0,11) == "pnpservopos") {
    pnpservo.write(com.substring(12).toInt());
  } 
   else if (com.substring(0,14) == "solderservopos") {
    //Serial.println(com.substring(14+1).toInt());    
    solderservo.write(com.substring(14+1).toInt());
  }  
   return currpos;
}

int read_temp()
{
   int rawtemp = analogRead(THERMISTOR_PIN);
   int current_celsius = 0;

   byte i;
   for (i=1; i<NUMTEMPS; i++)
   {
      if (temptable[i][0] > rawtemp)
      {
         int realtemp  = temptable[i-1][1] + (rawtemp - temptable[i-1][0]) * (temptable[i][1] - temptable[i-1][1]) / (temptable[i][0] - temptable[i-1][0]);

         if (realtemp > 255)
            realtemp = 255; 

         current_celsius = realtemp;

         break;
      }
   }

   // Overflow: We just clamp to 0 degrees celsius
   if (i == NUMTEMPS)
   current_celsius = 0;

   return current_celsius;
}

void moveacc(long int inc, int spd, int acc){
 //speed is in steps per second 
 stepper.setCurrentPosition(0);
 stepper.setAcceleration(acc);
 while(stepper.currentPosition() != inc){
    stepper.setSpeed(spd);
    stepper.runSpeed();
 }
}