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