a4ae71287c
The bas code to be committed. Allows for serial communication and control, PID loop to control pressure, and heart rate control.
120 lines
3.1 KiB
C++
120 lines
3.1 KiB
C++
#include <Arduino.h>
|
|
#include <PID_v1.h>
|
|
|
|
#define pressurePin A0 // Get input from pressure sensor
|
|
#define motorPin 3 // control pump motor speed
|
|
#define dirPin 6
|
|
#define valvePin1 2 // control forward valve
|
|
#define valvePin2 4 // control return valve
|
|
|
|
double motorSpeed = 100; // 0 to 255
|
|
int heartRate = 0; // Controls the speed of the valve actuation (Typically 50-120)
|
|
double pressure = 0; // pressure read from transducer in kPa
|
|
double targetPressure = 0; //target pressure the system should be at in kPa. (Typically 5-25kPa)
|
|
String data = "";
|
|
|
|
PID pressurePID(&pressure, &motorSpeed, &targetPressure, 30, 5, 1, DIRECT);
|
|
|
|
void setMotorSpeed(){
|
|
pressure = (analogRead(pressurePin)/409.5)*6.89476; // 0 - 4095. (0 - 10 PSI)
|
|
pressurePID.Compute();
|
|
if(heartRate <= 0){
|
|
analogWrite(motorPin, 0);
|
|
}
|
|
else{
|
|
analogWrite(motorPin, (int)motorSpeed);
|
|
}
|
|
}
|
|
|
|
void heartBeat(){ //turns the valves on and off to simulate one heart beat
|
|
if(heartRate <= 0){return;}
|
|
int delayTime = (60000)/(heartRate*2); // half the time of a single heartbeat in ms
|
|
digitalWrite(valvePin1, HIGH);
|
|
//delay(delayTime);
|
|
unsigned long startTime = millis();
|
|
while(millis()-startTime < delayTime){
|
|
setMotorSpeed();
|
|
printPID();
|
|
}
|
|
digitalWrite(valvePin1, LOW);
|
|
digitalWrite(valvePin2, HIGH);
|
|
//delay(delayTime);
|
|
startTime = millis();
|
|
while(millis()-startTime < delayTime){
|
|
setMotorSpeed();
|
|
printPID();
|
|
}
|
|
digitalWrite(valvePin2, LOW);
|
|
}
|
|
|
|
void processString(){
|
|
if (data.substring(0, 5) == "BEAT "){
|
|
heartRate = data.substring(5, data.length()).toInt();
|
|
Serial.print("Heart Rate set to ");
|
|
Serial.print(heartRate);
|
|
Serial.println(" BPM.");
|
|
}
|
|
else if (data.substring(0, 5) == "PRSR "){
|
|
targetPressure = data.substring(5, data.length()).toDouble();
|
|
Serial.print("Target Pressure set to ");
|
|
Serial.print(targetPressure);
|
|
Serial.println(" kPa.");
|
|
}
|
|
else if (data.substring(0, 5) == "FILL "){
|
|
int fillTime = data.substring(5, data.length()).toInt();
|
|
digitalWrite(valvePin1, HIGH);
|
|
digitalWrite(valvePin2, HIGH);
|
|
analogWrite(motorPin, 255);
|
|
delay(fillTime*1000);
|
|
digitalWrite(valvePin1, LOW);
|
|
digitalWrite(valvePin2, LOW);
|
|
}
|
|
}
|
|
|
|
void getData(){
|
|
//Serial.println("data recieved");
|
|
data = "";
|
|
while(Serial.available() > 0){
|
|
char c = Serial.read();
|
|
if(c != '\n'){
|
|
data += c;
|
|
}
|
|
delay(2);
|
|
}
|
|
Serial.println(data);
|
|
}
|
|
|
|
void printPID(){
|
|
Serial.print("Current Pressure: ");
|
|
Serial.print(pressure);
|
|
Serial.print(", Target Pressure: ");
|
|
Serial.print(targetPressure);
|
|
Serial.print(", Motor Speed:");
|
|
Serial.println(motorSpeed);
|
|
}
|
|
|
|
void setup() {
|
|
// put your setup code here, to run once:
|
|
pinMode(pressurePin, INPUT);
|
|
pinMode(motorPin, OUTPUT);
|
|
pinMode(dirPin, OUTPUT);
|
|
pinMode(valvePin1, OUTPUT);
|
|
pinMode(valvePin2, OUTPUT);
|
|
digitalWrite(dirPin, LOW);
|
|
Serial.begin(115200);
|
|
pressurePID.SetOutputLimits(0, 255);
|
|
pressurePID.SetMode(AUTOMATIC);
|
|
}
|
|
|
|
void loop() {
|
|
// put your main code here, to run repeatedly:
|
|
heartBeat();
|
|
if(Serial.available()){
|
|
getData();
|
|
processString();
|
|
}
|
|
setMotorSpeed();
|
|
printPID();
|
|
|
|
}
|