Servo Depth controller

I have had an idea for a depth control system based on a servo, I have tested a prototype and it appears to move as expected but i havent tested it in the water yet.

basically the idea is to take the control board from a servo, cut the signal pin to the potentiometer or remove it completely, replace it with an analogue pressure sensor and replace the motor with a 2 way pump. so as far as the servo is concerned its just trying to move the servo horn to a selected position where in reality it should move the sub down until the pressure on the sensor creates a voltage output that matches the position the servo controller is looking for. I have tested it using compressed air on the sensor and the servo motor changes direction to try and compensate for the changing pressure. My next test with be with a test tank and a peristaltic pump to see if it can hold depth.

so in theory 0-100 on the servo input should equal a around 0-3.5 meters using a 5psi sensor. but since they are 5v sensors and the servos operate on 3.3v internally then it will be more like 0-2m without a voltage divider.

This could be scaled up by adding some transistors to the servo letting it operate a 12v pump.

Tested it with a mosfet connected to a large air pump and it does try and hold a set pressure.

I have made another test version but this time using a L298N motor driver connected via some diodes but you can connect the servo controller directly if your using a 2 way pump. its got a slight leak in it so it keeps adding air to regulate the pressure. Unfortunatly the sensor i ordered was faulty so im using a spare 100psi sensor to test so i wont be able to do a water test until i get another 5psi sensor.

I like the idea of using a servo.

Your pressure sensor looks very rugged but quite large.

I had some success with the one here:

https://www.theassociationofmodelsubmariners.com/t1981-depth-sensor-lora-telemetry?highlight=depth

Although to be fair, I haven't tried it again recently. (I always worry that damp in the pressure sensor will not be good for it.)

Its an automotive sensor that I had sitting around that I could use to test the concept.

This is the sensor I'm planning on getting for use for testing in water as its much smaller and has a smaller hose fitting. To use the servo board it needs to be an analogue sensor so the sensor you used to wouldn't work.

https://aliexpress.com/item/1005004981423661.html

to test it in the water im planning on building a very simple ROV consisting of a gopro with a bag attached to it. In theory the pressure required to pump the air down to the depth of the bag equals the pressure of the water at that depth so by regulating the pressure of the air getting pumped down I can monitor and control its depth like an old style diving bell.

My only worry is that there is no PID, so if you had a really fast diving sub then it might end up yoyoing up and down at the target depth, I think for this to work correctly its going to need some manual input to bring it down slowly or for it to be brought down in steps using something like a 6 position switch, by keeping its vertical speed low its less lightly to go out of control.

I have done some more testing with a 2 way pump and a juice bottle as a pressure vessel. You can see that it will turn the pump in both directions to keep the pressure at the set level.

For this to work with the diving bell idea, the pump would be reversed, so we would extract air to increase the pressure since that will cause the sub to dive increasing the water pressure on the bag. It should work as long as you don't remove all the air from the system since its relying on the air being compressed to measure its depth.

It might be worth looking at a way of externally controlling the speed of the pump, that way it's fast when you want to surface or trying to submerge but it runs much slower when it's submerged, that way it's less lightly to overcorrect. I dont even think it would have to control its speed, even an external timer that just switches the motor driver mosfet off and on, even 1hz would work since its a relatively slow responding system.

I have ordered one of these sensors for testing in the water but its going to be about a week before it gets here.
https://www.ebay.co.uk/itm/295015225661

I found an old analogue Pitot tube from a flight control system that i could use for some more testing. the sensor only works up to less than 1psi so it would only work down to a couple of inches but you can see I can essentially dial a depth.

pressure sensor arrived, turns out I ordered the wrong one as it only outputs upto 100mv not 5v so I ordered another one that should work.

https://www.aliexpress.com/item/1005003768236942.html

Looks very suitable.

I have been doing some testing with the servo depth controller and have had promising results. 

I am just using the pitot tube sensor. I realized that I'm not planning on diving more than periscope depth, so using it would be ideal. 

I just wired a small 5v peristaltic pump to the servo board and added a drinks pouch as a bladder and threw it in. if you were only using this for trim control then you could just add the little pump along side your main ballast pump.

I added a variable resistor between the sensor and servo board so I could adjust its range. 

here it is holding periscope depth in the bath. the idea is to have a fpv camera where the screwdriver is so its the only thing above the surface. I was using the screw driver as a weight so I could set a depth then add or remove the weight to see how it reacted and how long it took to go back to where it was set for.

this is the little pump im using for trim, its small enough that i can wire it directly to a standard servo board without needing the extra motor driver. They are not fast pumps but you dont need a fast pump for just trimming the sub for depth control. you could use this system along side your regular large pump so you can manually surface and dive and this will just do the fine trim.

https://www.aliexpress.com/item/1005005364863545.html

This is the sensor im using its a differential sensor so I have blocked one end of it.

https://www.ebay.co.uk/itm/266321787949

Been doing some more testing and it's not looking as good as it did the first time around. I think the screw driver was acting like a float in my last test adding stability. This time when I tried it I was getting oscillations. I think if it had some forward movements so there was water over the planes help stabilise it that it would work better but I'm not hopeful. the issue was it was getting out of phase with the submarine because the pump is so slow so it might work with a much more responsive system but mine is just too slow.

But all is not lost... there are other ways to make a servo. Plan b is a to make a custom servo driver then convert it. The main advantage is that it has PID control so it can be tuned to hold depth without oscillating.
Dual H bridge driver is £2.20 deliverd= https://www.ebay.co.uk/itm/154589607670
328p compatible chips for less than £1 https://www.aliexpress.com/item/1005004989412626.html
I have been using these chips in a few things now. the can run into some issues at 32mhz. just drop it to 16mhz and its fully compatible.

https://github.com/geofrancis/WiperMotorServo

Code:

#include <PID_v1.h>  //PID loop from http://playground.arduino.cc/Code/PIDLibrary

double Pk1 = 1;  //speed it gets there
double Ik1 = 0;
double Dk1 = 0;

double Setpoint1, Input1, Output1, Output1a;    // PID variables

PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT);    // PID Setup

volatile unsigned long pwm;
volatile boolean done;
unsigned long start;

int pot;

unsigned long currentMillis;

long previousMillis = 0;    // set up timers
long interval = 20;        // time constant for timers

void setup() {
  pinMode(2, INPUT);
  pinMode(A0, INPUT);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  attachInterrupt(0, timeit, CHANGE);
  
  Serial.begin(115200);

  PID1.SetMode(AUTOMATIC);              // PID Setup - trousers SERVO
  PID1.SetOutputLimits(-255, 255);
  PID1.SetSampleTime(20);

}


void timeit() {
    if (digitalRead(2) == HIGH) {
      start = micros();
    }
    else {
      pwm = micros() - start;
      done = true;
    }
  }


void loop() {

  currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {  //start timed event
      previousMillis = currentMillis;

      pot = analogRead(A0);
      Serial.print(pot);
      Serial.print(" , ");
      Serial.print (pwm);
      Serial.print(" , ");

      Setpoint1 = map(pwm,1000,2000,-255,255);
      Input1 = map(pot,0,1023,-255,255);
      PID1.Compute();

      Serial.println(Output1);    

      if (Output1 > 0) {
        analogWrite(5, Output1);
        analogWrite(6, 0);
      }
      else if (Output1 < 0) {
        Output1a = abs(Output1);
        analogWrite(5, 0);
        analogWrite(6, Output1a);
      }

      
      
      if (!done)
            return;              
            done = false;  
    
  } // end of timed event

}

this video shows how to tune the PID for depth control

since im using Arduino and not limited to analogue sensors I have changed the code to use a HX710B pressure sensor since it gives out calibrated numbers not just a 0-1023 value it will be easier to tune as i can set depth by working out what the pressure should be at that depth. I am just waiting for the H bridge driver to arrive.

Code:

#include <PID_v1.h>  //PID loop from http://playground.arduino.cc/Code/PIDLibrary
#include "HX710B.h"


const int DOUT = 3;   //sensor data pin
const int SCLK  = 2;   //sensor clock pin
HX710B pressure_sensor;
int PWM_PIN = 4; //Define Digital PIN

int maxdepth = 0.3;//depth in meters 4m max with hx710B
double Pk1 = 30;  //speed it gets there
double Ik1 = 1; //long term correction
double Dk1 = 200; //slowing down so it doesnt not overshoot

double Setpoint1, Input1, Output1, Output1a;    // PID variables

PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT);    // PID Setup

volatile unsigned long pwm;
volatile boolean done;
unsigned long start;

int pot;
int depth;
int pwm1 = 1500;
unsigned long currentMillis;

long previousMillis = 0;    // set up timers
long interval = 20;        // time constant for timers

void setup() {

  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(PWM_PIN, INPUT);
  
  Serial.begin(115200);
  pressure_sensor.begin(DOUT, SCLK);
  
  PID1.SetMode(AUTOMATIC);              // PID Setup
  PID1.SetOutputLimits(-255, 255);
  PID1.SetSampleTime(20);

}


void loop() {
 pwm = pulseIn(PWM_PIN, HIGH); //Read PWM Pulse
 depth = map(pot,0.01,5.8,0,13.37);

 if (pwm > 1000);{
  Input1 = map(pot,0.01,(maxdepth * 1.42),-255,255);  
 }
 if (pwm <= 1000);{
  Input1 = 0;
  Serial.println("RC FAILSAFE.");
 }
  
 if (pressure_sensor.is_ready()) {
      pot = pressure_sensor.psi();
      
  } else {
    Serial.println("Pressure sensor not found.");
    pot = 0;
         }

    
  currentMillis = millis();
  if (currentMillis - previousMillis >= interval) {
    
    //start timed event
      previousMillis = currentMillis;
      Setpoint1 = map(pwm,1000,2000,-255,255);      
      PID1.Compute();

      if (Output1 > 0) {
        analogWrite(5, Output1);
        analogWrite(6, 0);
      }
      else if (Output1 < 0) {
        Output1a = abs(Output1);
        analogWrite(5, 0);
        analogWrite(6, Output1a);
      }
    
      Serial.print("PSI: ");
      Serial.println(pressure_sensor.psi());
      Serial.print(pot);
      Serial.print(" , ");      
      Serial.print (depth);
      Serial.print(" , ");
      Serial.print (pwm);
      Serial.print(" , ");
       Serial.println(Output1);    
          
      if (!done)
            return;              
            done = false;  
  }
  } // end of timed event








They are rated for 40KPa, that works out to 5.8psi or 13 feet deep, more than enough for any RC sub. im going to seal it with epoxy as I dont think its an absolute pressure sensor.
https://www.ebay.co.uk/itm/193482839640


I have updated the code, it now has RC and sensor failsafes and max depth setting.

on the calibrated digital devices, read the spec sheet carefully. i had to eliminate one because the amount of time required between reads was too great to be of any use.

ie i had to let the device sit for .5 seconds after a read for it to be ready for another read.

do you have any idea how much a sub can sink in .5 seconds?

tsenecal wrote:on the calibrated digital devices, read the spec sheet carefully.  i had to eliminate one because the amount of time required between reads was too great to be of any use.

ie i had to let the device sit for .5 seconds after a read for it to be ready for another read.

do you have any idea how much a sub can sink in .5 seconds?

I done some checking and the hx710b should be able to sample the sensor at 40hz thats fast enough for our purposes.

I have epoxied one to seal it, it should be dry by tomorrow and hopefully the H bridge driver will have arrived.

been doing some testing and i think there is some kind of incompatibility between the hx710b library and the pulsein used for reading RC input, whenever I connect it to a receiver it looses connection to the sensor.

geofrancis wrote:been doing some testing and i think there is some kind of incompatibility between the hx710b library and the pulsein used for reading RC input, whenever I connect it to a receiver it looses connection to the sensor.

you should not be using pulsein to read the pwm signal from the receiver.

two reasons, 1) it uses interrupts that others like to use, and 2) if you lose signal (quite possible in submarines) you will end up in a blocked state. bad for processing anything else.


https://www.arduino.cc/reference/en/libraries/servoinput/