vacuum control working

HX710B.cpp

@@ -0,0 +1,112 @@
+#include "HX710B.h"
+
+HX710B::HX710B(uint8_t sck_pin , uint8_t do_pin, uint8_t mode)
+{
+    this->SCK_pin = sck_pin;
+    this->DO_pin = do_pin;
+    this->mode = mode;
+}
+
+HX710B::~HX710B(){}
+
+void HX710B::halt_(unsigned long time_)
+{
+    if( this->delay_ == NULL )
+        delay(time_);
+        
+    else
+        this->delay_(time_);
+}
+
+unsigned long HX710B::getTick(void)
+{
+    if( this->tick != NULL )
+        return this->tick();
+    else    
+        return millis();
+}
+
+void HX710B::attachDelay(void (*delay__)(unsigned long ti))
+{
+    this->delay_ = delay__;
+}
+
+void HX710B::attachTick(unsigned long (*tick_)(void))
+{
+    this->tick = tick_;
+}
+
+uint8_t HX710B::init(void)
+{
+
+    if (this->SCK_pin == NO_PIN_DEFINED ||
+        this->DO_pin == NO_PIN_DEFINED )
+        return HX710B_ERROR;
+    pinMode(this->DO_pin, INPUT_PULLUP);    digitalWrite(this->DO_pin, HIGH);
+    pinMode(this->SCK_pin, OUTPUT);         digitalWrite(this->SCK_pin, HIGH);
+    for (uint8_t t_ = 0; t_ < this->mode; t_++)
+    {
+        NOP NOP NOP NOP NOP NOP NOP NOP NOP
+        digitalWrite(this->SCK_pin, LOW);
+        NOP NOP NOP NOP NOP NOP NOP NOP NOP
+        digitalWrite(this->SCK_pin, HIGH);
+    }
+    
+    read(NULL, 500UL);
+    this->halt_(500);
+    uint8_t ret = read(NULL, 100);
+    return ret;
+}
+
+uint8_t HX710B::setMode(uint8_t mode_, unsigned long timeout_)
+{
+    uint32_t val;
+    return read(&val, timeout_);
+}
+
+uint8_t HX710B::read(uint32_t *data, unsigned long timeout_)
+{
+    // exit sleep mode
+    digitalWrite(this->SCK_pin, LOW);
+    if (this->isReady(timeout_) == HX710B_OK)
+    {
+        uint8_t i = 0;
+        uint32_t raw = 0;
+        for (i = 0; i < this->mode; i++)
+        {
+            NOP NOP NOP NOP NOP NOP NOP NOP NOP
+            digitalWrite(this->SCK_pin, HIGH);
+            NOP NOP NOP NOP NOP NOP NOP NOP NOP
+            digitalWrite(this->SCK_pin, LOW);
+            if (i < 24)
+            {
+                raw = raw << 1;
+                if (digitalRead(this->DO_pin))
+                    raw = raw | 0x01;
+            }
+        }
+        // force the HX710B to enter the sleep mode
+        digitalWrite(this->SCK_pin, HIGH);
+        if( data != NULL)
+            *data = raw ^ 0x800000;
+        return HX710B_OK;
+    }
+    return HX710B_TIMEOUT;
+}
+
+uint8_t HX710B::isReady(unsigned long timeout_)
+{
+    unsigned long time_s = this->getTick();
+    while (digitalRead(this->DO_pin)!= LOW)
+    {
+        // preventive if counter is roll-over and start from beginning
+        unsigned long rollOver = this->getTick();
+        if (rollOver < time_s)
+            time_s = rollOver;
+        if (time_s - this->getTick() > timeout_)
+            return HX710B_TIMEOUT;
+        // need delay;
+        this->halt_(5);
+    }
+    return HX710B_OK;
+}

HX710B.h

@@ -0,0 +1,51 @@
+#ifndef _HX710B_H_
+#define _HX710B_H_
+
+#include "Arduino.h"
+#include "stdlib.h"
+#include "stdio.h"
+
+#define NOP             __asm__("nop\n\t");
+#define halt_1micros    NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP NOP
+
+#define HX710B_DIFF1    25      // data rate 10Hz
+#define HX710B_TEMP     26      // data rate 40Hz
+#define HX710B_DIFF2    27      // data rate 40Hz
+
+#define HX710B_DEFAULT_MODE     HX710B_DIFF2
+
+
+#define NO_PIN_DEFINED      255
+/* Status Reply */
+
+#define HX710B_OK       0x00U
+#define HX710B_ERROR    0x01U
+#define HX710B_BUSY     0x02U
+#define HX710B_TIMEOUT  0x03U
+
+class HX710B{
+
+    private :
+        uint8_t SCK_pin = NO_PIN_DEFINED,
+                DO_pin  = NO_PIN_DEFINED;
+        uint8_t mode;
+        void (*delay_)(unsigned long time_) = NULL;
+        unsigned long (*tick)(void) = NULL;
+        
+        void halt_ ( unsigned long time_);
+        unsigned long getTick(void);
+
+    public :
+        
+        HX710B(uint8_t sck_pin = NO_PIN_DEFINED, uint8_t do_pin = NO_PIN_DEFINED, uint8_t mode = HX710B_DEFAULT_MODE);
+        ~HX710B();
+        uint8_t init(void);
+        uint8_t setMode(uint8_t mode_, unsigned long timeout_ = 500UL);
+        uint8_t read(uint32_t *data, unsigned long timeout_ = 500UL);
+        uint8_t isReady(unsigned long timeout_ = 500UL);
+
+        void attachDelay(void (*delay__)(unsigned long ti));
+        void attachTick(unsigned long (*tick_)(void));
+};
+
+#endif

definitions.h

@@ -0,0 +1,35 @@
+#ifndef DEFINITION_H_
+#define DEFINITION_H_
+
+//Vacuum Pressure Sensor: HX710B
+#define PIN_PRESSURE1_OUT 20
+#define PIN_PRESSURE2_OUT 21
+#define PIN_PRESSURE3_OUT 22
+#define PIN_PRESSURE_SCK 15
+
+#define PIN_INPUTARM1 0
+#define PIN_INPUTARM2 1
+#define PIN_INPUTARM3 2
+#define PIN_INPUTAUX 3
+
+//Pins Connected to ULN2803
+#define PIN_OUT1 6 //PWM
+#define PIN_OUT2 7 //PWM
+#define PIN_OUT3 8 //PWM
+#define PIN_OUT4 9 //PWM
+#define PIN_OUT5 16 //Digital
+#define PIN_OUT6 17 //Digital
+#define PIN_OUT7 18 //Digital
+#define PIN_OUT8 19 //Digital
+
+#define PIN_BUTTON 23
+
+#define PIN_PUMP1 PIN_OUT1
+#define PIN_PUMP2 PIN_OUT3
+
+#define PIN_VALVE1 PIN_OUT5
+#define PIN_VALVE2 PIN_OUT7
+
+
+
+#endif

prosthesis_controller.ino

@@ -0,0 +1,42 @@
+#include "definitions.h"
+#include "vacuum.h"
+#include "servo.h"
+
+void setup() {
+  Serial.begin(115200);
+
+  pinMode(PIN_BUTTON,INPUT_PULLDOWN);
+
+  initVacuum();
+
+
+  pinMode(PIN_INPUTARM1,INPUT);
+}
+
+
+
+
+
+
+
+void loop() {
+  unsigned long loopmillis=millis();
+
+  bool vac=digitalRead(PIN_BUTTON); //Temporary Vacuum Button
+  setVacuum(vac);
+
+  loopVacuum(loopmillis);
+
+
+  //Print Debug Information
+  static unsigned long last_print=0;
+  if (loopmillis - last_print >100) {
+    last_print=loopmillis;
+    printVacuumValues();
+    
+    Serial.print(" Vac="); Serial.print(vac);
+    Serial.println();
+    Serial.println();
+  }
+
+}

servo.cpp

@@ -0,0 +1,2 @@
+#include "Arduino.h"
+#include "servo.h"

servo.h

@@ -0,0 +1,5 @@
+#ifndef SERVO_H_
+#define SERVO_H_
+
+
+#endif 

vacuum.cpp

@@ -0,0 +1,138 @@
+#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;
+}

vacuum.h

@@ -0,0 +1,20 @@
+#ifndef VACUUM_H_
+#define VACUUM_H_
+
+#include <PID_v1.h> //PID by Brett Beauregard 1.2.0 https://github.com/br3ttb/Arduino-PID-Library/tree/master
+#include "definitions.h"
+
+
+
+
+
+
+
+void initVacuum();
+void loopVacuum(unsigned long millis);
+void setVacuum(bool val);
+void printVacuumValues();
+long readHX710B(uint8_t outpin);
+
+
+#endif /* VACUUM_H_ */