138 lines
No EOL
3.8 KiB
C++
138 lines
No EOL
3.8 KiB
C++
#include "Arduino.h"
|
|
#include "vacuum.h"
|
|
|
|
|
|
double vacuumPressureRelease=8.5; //If pressure is below this value and vacuum is turned off, valves will open
|
|
double vacuumPressureReleasehysteresis=0.5; //hysteresis for release pressure
|
|
double vacuumPressureSuck=2.0;
|
|
|
|
double vacuum1=0; //Input
|
|
double vacuumPump1=0; //Output
|
|
double vacuumSetpoint1=vacuumPressureSuck;
|
|
|
|
double vacuum2=0; //Input
|
|
double vacuumPump2=0; //Output
|
|
double vacuumSetpoint2=vacuumSetpoint1;
|
|
|
|
bool vacuumEnabled=false;
|
|
|
|
double Kp=0.8, Ki=0.01, Kd=0.0;
|
|
PID pump1PID(&vacuum1, &vacuumPump1, &vacuumSetpoint1, Kp, Ki, Kd, REVERSE);
|
|
PID pump2PID(&vacuum2, &vacuumPump2, &vacuumSetpoint2, Kp, Ki, Kd, REVERSE);
|
|
|
|
void initVacuum() {
|
|
pinMode(PIN_PRESSURE1_OUT, INPUT); // Connect HX710 OUT
|
|
pinMode(PIN_PRESSURE2_OUT, INPUT); // Connect HX710 OUT
|
|
pinMode(PIN_PRESSURE_SCK, OUTPUT); // Connect HX710 SCK
|
|
|
|
pinMode(PIN_PUMP1, OUTPUT);
|
|
digitalWrite(PIN_PUMP1, LOW);
|
|
|
|
pinMode(PIN_PUMP2, OUTPUT);
|
|
digitalWrite(PIN_PUMP2, LOW);
|
|
|
|
pinMode(PIN_VALVE1,OUTPUT);
|
|
pinMode(PIN_VALVE2,OUTPUT);
|
|
|
|
pump1PID.SetOutputLimits(0.0, 1.0);
|
|
pump1PID.SetMode(AUTOMATIC);
|
|
|
|
pump2PID.SetOutputLimits(0.0, 1.0);
|
|
pump2PID.SetMode(AUTOMATIC);
|
|
|
|
}
|
|
|
|
|
|
void loopVacuum(unsigned long millis) {
|
|
static unsigned long last_pressurereading = 0;
|
|
if (millis - last_pressurereading > 10)
|
|
{
|
|
last_pressurereading=millis;
|
|
vacuum1 = readHX710B(PIN_PRESSURE1_OUT) / 1000000.0;
|
|
vacuum2 = readHX710B(PIN_PRESSURE2_OUT) / 1000000.0;
|
|
|
|
|
|
pump1PID.Compute();
|
|
|
|
if (vacuumEnabled) {
|
|
digitalWrite(PIN_VALVE1,LOW); //close valve
|
|
}else{
|
|
vacuumPump1=0.0; //turn off pump
|
|
if (!digitalRead(PIN_VALVE1) && vacuum1<vacuumPressureRelease-vacuumPressureReleasehysteresis) {
|
|
digitalWrite(PIN_VALVE1,HIGH); //open valve
|
|
}else if (digitalRead(PIN_VALVE1) && vacuum1>vacuumPressureRelease+vacuumPressureReleasehysteresis) {
|
|
digitalWrite(PIN_VALVE1,LOW); //close valve
|
|
}
|
|
|
|
}
|
|
|
|
if (vacuumEnabled) {
|
|
pump2PID.Compute();
|
|
digitalWrite(PIN_VALVE2,LOW); //close valve
|
|
}else{
|
|
vacuumPump2=0.0; //turn off pump
|
|
if (!digitalRead(PIN_VALVE2) && vacuum2<vacuumPressureRelease-vacuumPressureReleasehysteresis) {
|
|
digitalWrite(PIN_VALVE2,HIGH); //open valve
|
|
}else if (digitalRead(PIN_VALVE2) && vacuum2>vacuumPressureRelease+vacuumPressureReleasehysteresis) {
|
|
digitalWrite(PIN_VALVE2,LOW); //close valve
|
|
}
|
|
|
|
}
|
|
|
|
|
|
analogWrite(PIN_PUMP1,constrain(vacuumPump1,0.0,1.0)*255);
|
|
analogWrite(PIN_PUMP2,constrain(vacuumPump2,0.0,1.0)*255);
|
|
}
|
|
}
|
|
|
|
void setVacuum(bool val) {
|
|
if (val) { //True = turn off, False = Release
|
|
pump2PID.SetMode(AUTOMATIC);
|
|
vacuumSetpoint1=vacuumPressureSuck;
|
|
vacuumSetpoint2=vacuumSetpoint1;
|
|
vacuumEnabled=true;
|
|
}else{
|
|
pump2PID.SetMode(MANUAL);
|
|
vacuumEnabled=false;
|
|
}
|
|
}
|
|
|
|
void printVacuumValues() {
|
|
Serial.print(vacuum1, 3);
|
|
Serial.print(" -> ");
|
|
Serial.print(vacuumSetpoint1, 1);
|
|
Serial.print(" vacuumPump1="); Serial.print(vacuumPump1);
|
|
|
|
|
|
Serial.println();
|
|
Serial.print(vacuum2, 3);
|
|
Serial.print(" -> ");
|
|
Serial.print(vacuumSetpoint2, 1);
|
|
Serial.print(" vacuumPump2="); Serial.print(vacuumPump2);
|
|
}
|
|
|
|
long readHX710B(uint8_t outpin) {
|
|
//This function takes 95 microseconds
|
|
// pulse the clock line 3 times to start the next pressure reading
|
|
for (char i = 0; i < 3; i++) {
|
|
digitalWrite(PIN_PRESSURE_SCK, HIGH);
|
|
digitalWrite(PIN_PRESSURE_SCK, LOW);
|
|
}
|
|
while (digitalRead(outpin)) {} //TODO: add timeout
|
|
|
|
|
|
// read 24 bits
|
|
long result = 0;
|
|
for (int i = 0; i < 24; i++) {
|
|
digitalWrite(PIN_PRESSURE_SCK, HIGH);
|
|
digitalWrite(PIN_PRESSURE_SCK, LOW);
|
|
result = result << 1;
|
|
if (digitalRead(outpin)) {
|
|
result++;
|
|
}
|
|
}
|
|
|
|
// get the 2s compliment
|
|
result = result ^ 0x800000;
|
|
return result;
|
|
} |