Category Archives: Submarine 4.0

RC Submarine 4.0 – conclusion (10/10)

Here is the finished radio-controlled submarine.

Does it work? Yeah, it drives well under water. The automatic depth control really makes controlling it easy, as you can focus on pressing only forward/backward and left/right buttons and forget the dive/surface buttons. I’d say the controls are as good as in Submarine 2.0, which has been the best so far.

Pros? PID control works quite accurately. Seals were always watertight and the hull never leaked. Submarine top speed is the best so far. The main propeller is powerful. Magnetic couplings work more efficiently than before, because of silicone lubricant. More free space inside the hull because of tungsten pellets.

Cons? End caps are time consuming to close, because of the tight seals and the need to equalize internal pressure. Radio control range is short, only 4 meters. Turn propeller is a bit too weak. The submarine won’t drive straight when going top speed, probably because of the flat front. Tungsten pellets are expensive. Laser distance sensor is almost useless, because its range in clear water is only 0.5 meters. Tacho pulses are sometimes lost and you need to fix syringe position parameter. Complex to build.

Do I recommend building this? Not really. It is too complex and time consuming to build. It took me about 300 hours, although a lot was spent in development, mistakes and videography. Unless you want to try PID control specifically, I’d recommend Submarine 2.0 which is simpler and has good controls.

How to improve it? Make the end caps easier to install and add a locking mechanism. Change the radio control board to something more powerful. Remove laser distance sensor. Increase turn propeller size or gear ratio. Make the front more streamlined. Make the syringe position data reliable somehow.

Key features

Displacement: 2.4 kg
Test depth: 1.5 m
Radio control range: 4 m
Power consumption: 1.4 W idle, 5.6 W max
Battery runtime: 2 hours
Top speed: 0.6 knots (0.3 m/s, 1.1 km/h)

Parts

Hull: acrylic plastic cylinder (250x110x3mm)
End caps: SAN plastic lid (2mm thick), acrylic plastic cylinder (100x3mm)
Seal: o-ring 2.5 mm (NBR 70 shore)
Extra weight: tungsten pellets (2.5 mm diameter, 1.58 kg)
Ballast tank: 60 ml syringe (Eotia marinade injector)
Ballast motor: Lego EV3 Medium Servo motor (45503)
Forward propeller: drone propeller (Diatone Bull Nose 4×4.5)
Forward motor: Lego PF L-motor (88003)
Turn propeller: Lego propeller 3 blade (6041)
Turn motor: Lego PF M-motor (8883)
Magnets: K&J Magnetics D38-N52 neodymium magnet
Friction reduction: TapeCase 423-5 UHMW Tape, silicon spray
Motor driver: Pololu 2130 DRV8833 Dual H-bridge
Radio control: 27 MHz controller dissembled from a toy submarine (no-name chinese Mini U Boat)
Pressure sensor: Honeywell SSCMANV030PA2A3 2 bar
Laser distance sensor: SparkFun TFMini-S Micro
Computer: Raspberry Pi Zero 2 W
Power supply: Lego Rechargeable Battery Box 9V (8878)
Voltage regulator: Pololu 2123 S7V8F5 5V

Lego parts

1x 8878 Power Functions Rechargeable Battery Box
1x 99455 Electric, Motor EV3, Medium
1x 45514 Cable Pack EV3
1x 88003 Power Functions L-Motor
1x 8883 Power Functions M-Motor
2x 8886 Power Functions Extension Wire (20cm)
1x 6041 Propeller 3 Blade 3 Diameter with Axle Hole
1x col154 Sea Captain, Series 10 (Minifigure Only without Stand and Accessories)
1x 4079b Minifigure, Utensil Seat / Chair 2 x 2 with Center Sprue Mark
1x 3069bp68 Tile 1 x 2 with Groove with Red and Yellow Controls and Two White Stripes on Left Pattern
1x 4175 Ladder 1 1/2 x 2 x 2
2x 3649 Technic, Gear 40 Tooth
4x 3648 Technic, Gear 24 Tooth (2nd Version - 1 Axle Hole)
2x 6589 Technic, Gear 12 Tooth Bevel
4x 10928 Technic, Gear 8 Tooth with Dual Face
1x 4716 Technic, Gear Worm Screw, Long
6x 3743 Technic, Gear Rack 1 x 4
1x 6588 Technic, Gearbox 2 x 4 x 3 1/3
1x 48496 Technic, Pin Connector Toggle Joint Smooth Double with 2 Pins
2x 32530 Technic, Pin Connector Plate 1 x 2 x 1 2/3 with 2 Holes (Double on Top)
2x 62462 Technic, Pin Connector Round 2L with Slot (Pin Joiner Round)
2x 6558 Technic, Pin 3L with Friction Ridges
28x 4459 Technic, Pin with Friction Ridges without Center Slots
1x 32002 Technic, Pin 3/4
6x 4274 Technic, Pin 1/2 without Friction Ridges
6x 3713 Technic Bush
3x 4265c Technic Bush 1/2 Smooth
22x 4265a Technic Bush 1/2 Toothed, Four Interior Ridges
1x 3737 Technic, Axle 10L
2x 44294 Technic, Axle 7L
2x 3706 Technic, Axle 6L
5x 3705 Technic, Axle 4L
2x 18651 Technic, Axle 2L with Pin with Friction Ridges
4x 43093 Technic, Axle 1L with Pin with Friction Ridges
1x 3703 Technic, Brick 1 x 16 with Holes
1x 2730 Technic, Brick 1 x 10 with Holes
1x 3894 Technic, Brick 1 x 6 with Holes
1x 3701 Technic, Brick 1 x 4 with Holes
1x 32525 Technic, Liftarm Thick 1 x 11
1x 32524 Technic, Liftarm Thick 1 x 7
1x 32316 Technic, Liftarm Thick 1 x 5
4x 32523 Technic, Liftarm Thick 1 x 3
1x 43857 Technic, Liftarm Thick 1 x 2
2x 18654 Technic, Liftarm Thick 1 x 1 (Spacer)
4x 32526 Technic, Liftarm, Modified Bent Thick L-Shape 3 x 5
1x 32140 Technic, Liftarm, Modified Bent Thick L-Shape 2 x 4
4x 60484 Technic, Liftarm, Modified T-Shape Thick 3 x 3
3x 32056 Technic, Liftarm, Modified Bent Thin L-Shape 3 x 3
2x 41677 Technic, Liftarm Thin 1 x 2 - Axle Holes
2x 4032 Plate, Round 2 x 2 with Axle Hole
3x 3020 Plate 2 x 4
1x 3021 Plate 2 x 3
2x 60479 Plate 1 x 12
1x 4477 Plate 1 x 10
3x 3666 Plate 1 x 6
1x 3710 Plate 1 x 4
2x 3023 Plate 1 x 2
1x 3069 Tile 1 x 2
1x 3003 Brick 2 x 2
1x 3001 Brick 2 x 4
1x 3004 Brick 1 x 2

Cost

236 EUR tungsten pellets
100 EUR Lego rechargeable battery box
55 EUR laser distance sensor TFMini-S Micro
36 EUR acrylic plastic hull
35 EUR pressure sensor SSCMANV030PA2A3
33 EUR radio control (mini u-boat)
28 EUR Raspberry Pi Zero 2 W
25 EUR Lego Medium motor EV3
24 EUR magnets
22 EUR crimp tool
21 EUR Lego M-Motor
20 EUR pin header connector kit
20 EUR voltage regulator
18 EUR o-ring cord
18 EUR Lego L-Motor
10 EUR motor drivers
6 EUR Lego captain
5 EUR clear case for Raspberry Pi Zero
5 EUR SAN plastic lid
4 EUR syringe
3 EUR wire
1 EUR main propeller
50 EUR other Lego parts
TOTAL COST: 800 EUR

Interior images

Testing images

An orange camera is on-board for shooting video from the river.

Another under water shot from the river.

Video

Here is a YouTube video of this project. From 2:30 onward you find test footage from the water container, swimming pool, and river.

And here is the entire 20 minute dive in the river.

Code

Python implementation and logs for download:
https://mega.nz/folder/wqhhxJoI#cHnVI6NjaPkfZcdZvFZZ0Q
or
https://www.dropbox.com/sh/u8a45maz3brrwrw/AADbUx9VZEw5UJPny1bJI-Lwa

Python implementation for the submarine:

#################################################
# Submarine 4.0
# Created by: Brick Experiment Channel
# Hardware:
#    Raspberry Pi Zero 2 W
#    Honeywell SSCMANV030PA2A3 pressure sensor
#    SparkFun TFMini-S Micro laser distance sensor
#    Pololu 2130 DRV8833 motor drivers
#    Pololu 2123 S7V8F5 5V Voltage regulator
#    27MHz radio controller from a toy submarine
#    Lego EV3 Medium motor for syringe ballast tank
#    Lego PF Large motor for forward propeller
#    Lego PF Medium motor for turn propeller
#    
#################################################

#settings
SYRINGE_POS_MIN     = 3     #syringe ballast min pos [ml]
SYRINGE_POS_MAX     = 45    #syringe ballast max pos [ml]
SYRINGE_TACHO_COUNT = 19000 #tacho count from syringe min to max pos
SYRINGE_HYSTERESIS  = 360   #syringe+gearbox backlash/hysteresis [tacho counts]
SYRINGE_DEADBAND    = 0.5   #deadband to reduce motor wear [ml]
SYRINGE_NEUTRAL_POS = 30    #neutral buoyancy position [ml]
DEPTH_FILTER        = 0.90  #pressure sensor depth EMA filter ratio [0...1]
DISTANCE_FILTER     = 0.95  #laser distance EMA filter ratio [0...1]
KD_FILTER           = 0.95  #derivative EMA filter ratio [0...1]
MAX_DEPTH           = 4     #maximum limit for depth control [m]
MAX_DISTANCE        = 1     #maximum limit for laser distance [m]
KP                  = 30    #PID proportional factor
KI                  = 1     #PID integral factor
KD                  = 200   #PID derivative factor
SLEEP_TIME          = 20    #main loop sleep [ms]

#################################################

from time import sleep, perf_counter_ns
from datetime import datetime
import math
import signal
import smbus
import configparser
from serial import Serial
from gpiozero import DigitalInputDevice, DigitalOutputDevice, PWMOutputDevice, Button
from gpiozero import LoadAverage, CPUTemperature

#syringe control mode
MODE_DIRECT      = 0  #no PID (radio buttons control syringe directly)
MODE_PID         = 1  #PID over pressure sensor (buttons change target depth)
MODE_PID_LASER   = 2  #PID over laser sensor (buttons change target distance to sea floor)

controlMode = MODE_DIRECT
targetDepth = 0.10     #[m]
targetDistance = 0.10  #[m]

#read config file
configFile = open("submarine_4.ini", "r+")
config = configparser.ConfigParser()
config.read_file(configFile)
tachoCount = int(config['default']['tachoCount'])

storedTachoCount = tachoCount
def writeConfigFile():
    global configFile
    global config
    global tachoCount
    global storedTachoCount
    if tachoCount != storedTachoCount:
        storedTachoCount = tachoCount
        config['default'] = {}
        config['default']['tachoCount'] = str(tachoCount)
        configFile.seek(0)
        config.write(configFile)
        configFile.truncate()

#prepare pressure sensor
bus = smbus.SMBus(1)

def readPressureSensor():
    #read data
    PRESSURE_SENSOR_ADDR = 0x28
    data = bus.read_i2c_block_data(PRESSURE_SENSOR_ADDR, 0x00, 4)
    status = (data[0] & 0xC0) >> 6
    pressCounts = data[1] | ((data[0] & 0x3F) << 8)
    tempCounts = ((data[3] & 0xE0) >> 5) | (data[2] << 3)
    
    #pressure conversion
    P_MAX = 2  #[bar]
    P_MIN = 0  #[bar]
    O_MAX = 0.9 * pow(2,14)
    O_MIN = 0.1 * pow(2,14)
    pressure = (pressCounts - O_MIN) * (P_MAX - P_MIN) / (O_MAX - O_MIN) + P_MIN  #[bar]
    
    #temperature conversion
    T_MAX = 150  #[Celsius]
    T_MIN = -50  #[Celsius]
    T_COUNTS = pow(2,11) - 1
    temperature = tempCounts * (T_MAX - T_MIN) / T_COUNTS + T_MIN  #[Celsius]

    return pressure, temperature

#prepare laser distance sensor
serial = Serial("/dev/serial0", 115200, timeout=0.01)
serial.write([0x5A,0x05,0x05,0x06,0x6A])  #set unit to mm
serial.write([0x5A,0x06,0x03,0x64,0x00,0x39])  #set frame rate to 100 Hz
serial.write([0x5A,0x09,0x3B,0x00,0x00,0x00,0x00,0x00,0x62])  #set I/O mode to standard
sleep(0.1)
serial.reset_input_buffer()

def readLaserSensor():
    #skip old serial data
    FRAME_SIZE = 9
    while (serial.inWaiting() > FRAME_SIZE):
        serial.read()

    #read data
    data = serial.read(FRAME_SIZE)
    
    #parse data
    distance = float('nan')
    strength = float('nan')
    temperature = float('nan')
    if len(data) == FRAME_SIZE and data[0] == 0x59 and data[1] == 0x59:
        checksum = 0
        for i in range(0, FRAME_SIZE - 1):
            checksum = checksum + data[i]
        checksum = checksum & 0xFF
        if checksum == data[FRAME_SIZE - 1]:
            distance = data[2] | data[3] << 8  #[mm]
            strength = data[4] | data[5] << 8  #[0-65536]
            temperature = data[6] | data[7] << 8
            
            distance = distance / 1000   #[m]
            temperature = temperature / 8 - 256  #[C]
    
    #scale for water
    SPEED_IN_AIR = 299000  #[km/s]
    SPEED_IN_WATER = 225000  #[km/s]
    distance *= SPEED_IN_WATER / SPEED_IN_AIR
    
    return distance, strength, temperature

#prepare motor drivers
motorForward = PWMOutputDevice(22)
motorForward.frequency = 1000
motorBackward = PWMOutputDevice(27)
motorBackward.frequency = 1000
motorLeft = DigitalOutputDevice(17)
motorRight = DigitalOutputDevice(18)
motorDive = DigitalOutputDevice(13)
motorSurface = DigitalOutputDevice(12)

#prepare radio control
radioPower = DigitalOutputDevice(25)
buttonForward = Button(24, hold_time=1.0)
buttonBackward = Button(23, hold_time=1.0)
buttonLeft = Button(9)
buttonRight = Button(10)
buttonDive = Button(11, bounce_time=None, hold_time=1.0, hold_repeat=True)
buttonSurface = Button(8, bounce_time=None, hold_time=1.0, hold_repeat=True)

def changeTargetDepth(value):
    global controlMode
    global targetDepth
    global targetDistance
    if controlMode == MODE_PID:
        targetDepth += value
        if targetDepth < 0:
            targetDepth = 0
        if targetDepth > MAX_DEPTH:
            targetDepth = MAX_DEPTH
        print("targetDepth=%.0f cm" % (targetDepth*100))
    elif controlMode == MODE_PID_LASER:
        targetDistance -= value
        if targetDistance < 0.1:
            targetDistance = 0.1
        if targetDistance > MAX_DISTANCE:
            targetDistance = MAX_DISTANCE
        print("targetDistance=%.0f cm" % (targetDistance*100))
def buttonDive_pressed():
    changeTargetDepth(0.1)
def buttonDive_held():
    changeTargetDepth(0.5)
def buttonSurface_pressed():
    changeTargetDepth(-0.1)
def buttonSurface_held():
    changeTargetDepth(-0.5)
def buttonForward_pressed():
    motorForward.value = 0.6
def buttonForward_held():
    motorForward.value = 1
def buttonForward_released():
    motorForward.value = 0
def buttonBackward_pressed():
    motorBackward.value = 0.6
def buttonBackward_held():
    motorBackward.value = 1
def buttonBackward_released():
    motorBackward.value = 0
def buttonLeft_pressed():
    motorLeft.value = 1
def buttonLeft_released():
    motorLeft.value = 0
def buttonRight_pressed():
    motorRight.value = 1
def buttonRight_released():
    motorRight.value = 0

buttonForward.when_pressed   = buttonForward_pressed
buttonForward.when_held      = buttonForward_held
buttonForward.when_released  = buttonForward_released
buttonBackward.when_pressed  = buttonBackward_pressed
buttonBackward.when_held     = buttonBackward_held
buttonBackward.when_released = buttonBackward_released
buttonLeft.when_pressed      = buttonLeft_pressed
buttonLeft.when_released     = buttonLeft_released
buttonRight.when_pressed     = buttonRight_pressed
buttonRight.when_released    = buttonRight_released
buttonDive.when_pressed      = buttonDive_pressed
buttonDive.when_held         = buttonDive_held
buttonSurface.when_pressed   = buttonSurface_pressed
buttonSurface.when_held      = buttonSurface_held

#prepare tachometers
tachoPower = DigitalOutputDevice(20)
tachoA = DigitalInputDevice(16)
tachoB = DigitalInputDevice(19)

tachoAValue = tachoA.value
tachoBValue = tachoB.value
def tachoA_rising():
    global tachoCount
    global tachoAValue
    global tachoBValue
    tachoAValue = 1
    if tachoBValue == 0:
        #A in rising edge and B in low value
        #  => direction is clockwise (shaft end perspective)
        tachoCount += 1
    else:
        tachoCount -= 1
def tachoA_falling():
    global tachoAValue
    tachoAValue = 0
def tachoB_rising():
    global tachoCount
    global tachoAValue
    global tachoBValue
    tachoBValue = 1
    if tachoAValue == 0:
        tachoCount -= 1
    else:
        tachoCount += 1
def tachoB_falling():
    global tachoBValue
    tachoBValue = 0

tachoA.when_activated   = tachoA_rising
tachoA.when_deactivated = tachoA_falling
tachoB.when_activated   = tachoB_rising
tachoB.when_deactivated = tachoB_falling

tachoPower.value = 1

#exit program when Ctrl-C is pressed
exitRequested = False
def sigintHandler(sig, frame):
    print("Ctrl-C pressed, exit program")
    global exitRequested
    exitRequested = True
signal.signal(signal.SIGINT, sigintHandler)
signal.signal(signal.SIGTERM, sigintHandler)

def sign(x):
    if x > 0: return 1
    elif x < 0: return -1
    else: return 0

print("program started")
sleep(0.1)

startTime = perf_counter_ns()
prevLoopTime = perf_counter_ns()
prevModeButtonTime = perf_counter_ns()
prevRadioResetTime = perf_counter_ns()
prevTaskTime = perf_counter_ns()
prevLogTime = perf_counter_ns()
prevPrintTime = perf_counter_ns()
trueTachoCount = 0
depthZeroPoint = float('nan')
depthFiltered = 0
laserDistanceValidTime = 0
laserDistanceFiltered = 0
modeButtonStep = 0
error = 0
integral = 0
derivative = 0
PIDoutput = 0
prevError = float('nan')
prevDerivative = 0
syringeTargetPos = 0
deadbandDir = 0
cpuTemp = float('nan')
logFile = None
logData = []
loopCount = 0

while not exitRequested:
    timeDelta = (perf_counter_ns() - prevLoopTime) / 1e9  #[sec]
    prevLoopTime = perf_counter_ns()
    secondsSinceStart = (perf_counter_ns() - startTime) / 1e9
    
    #calculate syringe position
    if tachoCount > trueTachoCount:
        trueTachoCount = tachoCount
    elif tachoCount < trueTachoCount - SYRINGE_HYSTERESIS:
        trueTachoCount = tachoCount + SYRINGE_HYSTERESIS
    syringePos = SYRINGE_POS_MIN + SYRINGE_POS_MAX * \
                 trueTachoCount / SYRINGE_TACHO_COUNT  #[ml]
    
    #read pressure sensor
    pressure, pressureSensorTemp = readPressureSensor()
    
    #calculate depth
    GRAVITY = 9.80665  #[m/s2]
    WATER_DENSITY = 998  #fresh water at 20 Celsius [kg/m3]
    pressurePa = pressure * 100000  #[Pa]
    depth = pressurePa / (GRAVITY * WATER_DENSITY)  #[m]
    
    #adjust for depth zero point
    if math.isnan(depthZeroPoint): depthZeroPoint = depth
    if syringePos <= 0:
        #when syringe is empty, assume we are in surface and start adjusting
        depthZeroPoint = 0.01 * depth + 0.99 * depthZeroPoint
    depth -= depthZeroPoint
    
    #filter depth (exponential moving average)
    depthFiltered = (1 - DEPTH_FILTER) * depth + DEPTH_FILTER * depthFiltered
    
    #read laser sensor
    laserDistance, laserStrength, laserTemp = readLaserSensor()
    
    #fix laser data for close distance (when sensor says 0.05m it is actually 0.10m)
    laserDistanceFixed = laserDistance
    if laserDistanceFixed < 0.25:
        laserDistanceFixed += 0.5 * (0.25 - laserDistanceFixed) * \
                              math.sqrt(laserDistanceFixed / 0.25)
    
    #omit unreliable laser data
    if laserStrength < 300 or laserDistance > 10:
        laserDistanceFixed = float('nan')
        if (perf_counter_ns() - laserDistanceValidTime) / 1e9 >= 1.0:
            laserDistanceFiltered = float('nan')
    else:
        if math.isnan(laserDistanceFiltered):
            laserDistanceFiltered = laserDistanceFixed
        laserDistanceValidTime = perf_counter_ns()
    
    #filter laser distance (exponential moving average)
    if not math.isnan(laserDistanceFixed):
        laserDistanceFiltered = (1 - DISTANCE_FILTER) * laserDistanceFixed + \
                                DISTANCE_FILTER * laserDistanceFiltered

    #calculate syringe neutral position
    #  hull compression compensation (15 ml at 100 cm depth)
    syringeNeutralPos = SYRINGE_NEUTRAL_POS
    if depthFiltered > 1:
        syringeNeutralPos -= 15
    elif depthFiltered > 0:
        syringeNeutralPos -= math.sqrt(depthFiltered) * 15 / 1

    #change depth control mode
    #  triggered by button sequences
    #    MODE_DIRECT    = Surface-Dive-Surface
    #    MODE_PID       = Surface-Dive-Right
    #    MODE_PID_LASER = Surface-Dive-Left
    changeMode = False
    if buttonSurface.value == 1:
        if modeButtonStep == 0:
            prevModeButtonTime = perf_counter_ns()
            modeButtonStep = 1
        if modeButtonStep == 2:
            modeButtonStep = 3
            controlMode = MODE_DIRECT
            print("MODE_DIRECT")
    if buttonDive.value == 1:
        if modeButtonStep == 1:
            modeButtonStep = 2
    if buttonRight.value == 1:
        if modeButtonStep == 2:
            modeButtonStep = 3
            controlMode = MODE_PID
            print("MODE_PID")
    if buttonLeft.value == 1:
        if modeButtonStep == 2:
            modeButtonStep = 3
            controlMode = MODE_PID_LASER
            print("MODE_PID_LASER")
    if modeButtonStep != 0 and (perf_counter_ns() - prevModeButtonTime) / 1e9 >= 1.5:
        modeButtonStep = 0

    #control syringe motor
    syringeMotorCtrl = 0
    if controlMode == MODE_DIRECT:
        if buttonDive.value == 1: syringeMotorCtrl = 100
        elif buttonSurface.value == 1: syringeMotorCtrl = -100
    
    elif controlMode == MODE_PID:
        if targetDepth <= 0:
            #go to surface
            if syringePos > 0:
                syringeMotorCtrl = -100
            else:
                syringeMotorCtrl = 0
        else:
            #PID controller
            error = targetDepth - depthFiltered
            integral += error * timeDelta
            if math.isnan(prevError) or buttonDive.value == 1 or buttonSurface.value == 1:
                prevError = error
            derivative = (error - prevError) / timeDelta
            derivative = (1 - KD_FILTER) * derivative + KD_FILTER * prevDerivative
            PIDoutput = KP * error + KI * integral + KD * derivative
            prevError = error
            prevDerivative = derivative
            
            #move syringe to the target position
            syringeTargetPos = syringeNeutralPos + PIDoutput  #[ml]
            syringeMotorCtrl = syringeTargetPos - syringePos
    
    elif controlMode == MODE_PID_LASER:
        if depthFiltered >= MAX_DEPTH:
            #limit to max depth
            syringeMotorCtrl = -100
        
        elif math.isnan(laserDistanceFixed):
            #bottom too far for laser -> increase depth
            syringeMotorCtrl = 100
            
        else:
            #PID controller
            error = laserDistanceFiltered - targetDistance
            integral += error * timeDelta
            if math.isnan(prevError) or buttonDive.value == 1 or buttonSurface.value == 1:
                prevError = error
            derivative = (error - prevError) / timeDelta
            derivative = (1 - KD_FILTER) * derivative + KD_FILTER * prevDerivative
            PIDoutput = KP * error + KI * integral + KD * derivative
            prevError = error
            prevDerivative = derivative
            
            #move syringe to the target position
            syringeTargetPos = syringeNeutralPos + PIDoutput  #[ml]
            syringeMotorCtrl = syringeTargetPos - syringePos
    
    #syringe deadband limitation
    if abs(syringeMotorCtrl) > SYRINGE_DEADBAND:
        deadbandDir = sign(syringeMotorCtrl)
    if sign(syringeMotorCtrl) != sign(deadbandDir):
        deadbandDir = 0
    if deadbandDir == 0:
        syringeMotorCtrl = 0
    
    #syringe range limitation
    if syringePos <= SYRINGE_POS_MIN and syringeMotorCtrl < 0:
        syringeMotorCtrl = 0
    if syringePos >= SYRINGE_POS_MAX and syringeMotorCtrl > 0:
        syringeMotorCtrl = 0

    #drive syringe motor
    motorDive.value = (syringeMotorCtrl > 0)
    motorSurface.value = (syringeMotorCtrl < 0)
    
    #radio controller fix
    #  the radio board has an auto-surface safety feature
    #  after 10 secs of silence it will force the surface button on
    #  to fix it, we will quickly power off and on the radio every 9 secs
    if (buttonDive.value == 1 or buttonForward.value == 1 or
       buttonBackward.value == 1 or buttonLeft.value == 1 or buttonRight.value == 1):
        prevRadioResetTime = perf_counter_ns()
    secsSinceRadioReset = (perf_counter_ns() - prevRadioResetTime) / 1e9
    if secsSinceRadioReset > 9:
        radioPower.value = 0
        prevRadioResetTime = perf_counter_ns()
    else:
        radioPower.value = 1
    
    #low frequency tasks
    if (perf_counter_ns() - prevTaskTime) / 1e9 >= 1.0:
        prevTaskTime = perf_counter_ns()
        
        #get info from raspberry
        cpuTemp = CPUTemperature().temperature
        
        #write config file
        writeConfigFile()
    
    #debug data
    motorsState = (motorForward.value > 0) | (motorBackward.value > 0) << 1 | \
                  motorLeft.value << 2 | motorRight.value << 3 | \
                  motorDive.value << 4 | motorSurface.value << 5
    buttonsState = buttonForward.value | buttonBackward.value << 1 | \
                   buttonLeft.value << 2 | buttonRight.value << 3 | \
                   buttonDive.value << 4 | buttonSurface.value << 5
    
    #log data for post analysis
    logLine = [secondsSinceStart, pressure, depth, depthZeroPoint,
               depthFiltered, targetDepth, laserDistance,
               laserStrength, laserDistanceFiltered, targetDistance,
               tachoCount, trueTachoCount,
               syringePos, syringeTargetPos, syringeNeutralPos, 
               error, integral, derivative, PIDoutput, syringeMotorCtrl,
               controlMode, motorsState, buttonsState, radioPower.value,
               pressureSensorTemp, laserTemp, cpuTemp]
    logData.append([])
    for value in logLine:
        strValue = None
        if type(value) == float: strValue = str('%.5g' % value)
        else: strValue = str(value)
        logData[-1].append(strValue)
    
    #write log data to file
    if (perf_counter_ns() - prevLogTime) / 1e9 >= 1.0:
        prevLogTime = perf_counter_ns()
        if logFile == None:
            filename = "datalog_" + datetime.now().strftime("%Y%m%d_%H%M%S") + ".dat"
            logFile = open(filename, "w")
            logHeader = [["# Submarine 4.0 log"]]
            logHeader += [["# SETTINGS:","SYRINGE_POS_MIN",SYRINGE_POS_MIN,
                           "SYRINGE_POS_MAX",SYRINGE_POS_MAX,
                           "SYRINGE_TACHO_COUNT",SYRINGE_TACHO_COUNT,
                           "SYRINGE_HYSTERESIS",SYRINGE_HYSTERESIS,
                           "SYRINGE_DEADBAND",SYRINGE_DEADBAND,
                           "SYRINGE_NEUTRAL_POS",SYRINGE_NEUTRAL_POS,
                           "DEPTH_FILTER",DEPTH_FILTER,
                           "DISTANCE_FILTER",DISTANCE_FILTER,
                           "KD_FILTER",KD_FILTER,
                           "MAX_DEPTH",MAX_DEPTH,
                           "MAX_DISTANCE",MAX_DISTANCE,
                           "KP",KP,"KI",KI,"KD",KD,
                           "SLEEP_TIME",SLEEP_TIME]]
            logHeader += [["#"]]
            logHeader += [["secondsSinceStart","pressure","depth","depthZeroPoint",
                           "depthFiltered","targetDepth","laserDistance",
                           "laserStrength","laserDistanceFiltered","targetDistance",
                           "tachoCount","trueTachoCount",
                           "syringePos","syringeTargetPos","syringeNeutralPos",
                           "error","integral","derivative","PIDoutput","syringeMotorCtrl",
                           "controlMode","motorsState","buttonsState","radioPower",
                           "pressureSensorTemp","laserTemp","cpuTemp"]]
            logData = logHeader + logData
        for logLine in logData:
            for value in logLine:
                logFile.write(str(value) + ' ')
            logFile.write('\n')
        logFile.flush()
        logData.clear()
    
    #debug print
    if (perf_counter_ns() - prevPrintTime) / 1e9 >= 1.0:
        secondsSinceLastPrint = (perf_counter_ns() - prevPrintTime) / 1e9
        prevPrintTime = perf_counter_ns()
        loopInterval = secondsSinceLastPrint / loopCount * 1000
        loopCount = 0
        print("mode %.0f, depth %.1f, laser %.1f, tacho %.0f, syPos %.1f, loop %.1f"
              % (controlMode, depthFiltered*100, laserDistanceFiltered*100,
                 tachoCount, syringePos, loopInterval))
    
    sleep(SLEEP_TIME / 1000)
    loopCount += 1

#stop motors and power down components
motorForward.value = 0
motorBackward.value = 0
motorLeft.value = 0
motorRight.value = 0
motorDive.value = 0
motorSurface.value = 0
radioPower.value = 0
tachoPower.value = 0

writeConfigFile()
print("storedTachoCount=%i, syringePos=%.1f" % (storedTachoCount, syringePos))

print("program ended")

RC Submarine 4.0 – PID control (9/10)

4 Replies

Now I need to code a PID controller. The input will be depth from the pressure sensor, or distance from the laser. The output will be syringe ballast position. Syringe will change the weight of the submarine, that will introduce gravity force that changes the position. Position is measured by the sensors and looped back to the controller.

Main loop

The first thing to do is create a while loop. It will have many tasks:

calculate syringe position based on tacho count
read pressure sensor
read laser sensor
calculate PID control output
drive syringe motor
log data to file for later analysis

The code will be run on Raspberry Pi Zero 2 that has a powerful 1 GHz Quad-core ARM Cortex-A53 processor. One loop takes about 5 ms based on my measurements. From that about 1 ms is spent on a blocking call to read the pressure sensor.

I could run the loop as fast as possible as both sensors can handle 1000 Hz data rate. But that would consume batteries and heat up the Raspberry. Also, the submarine will move slowly, so timing won’t be an issue. I settled on using a 20 ms sleep in each loop.

With 20 ms sleep, loop interval is 25 ms and single core CPU load is 15% for the program.

PID control for depth

Here is the PID controller code in Python. Depth measurement comes in depthFiltered variable. Set point comes in targetDepth variable. Loop interval is stored in timeDelta. KP, KI and KD are the gain settings that will be tuned later.

#PID controller
error = targetDepth - depthFiltered
integral += error * timeDelta
if math.isnan(prevError) or buttonDive.value == 1 or buttonSurface.value == 1:
    prevError = error
derivative = (error - prevError) / timeDelta
derivative = 0.05 * derivative + 0.95 * prevDerivative
PIDoutput = KP * error + KI * integral + KD * derivative
prevError = error
prevDerivative = derivative

There are two abnormalities in the code. The first is conditions to set prevError = error. It’s purpose is to omit spikes in derivative that come from changes in the target depth. When pressing radio buttons, the target depth will have a sharp step in value, so the derivative would contain a sudden large impulse. The spikes would have a lasting effect since I’m using filter. The math.isnan(prevError) is just for testing purposes, so that I can start the program with an initial targetDepth value without derivative spikes.

Secondly there is an exponential moving average filter on the derivative. I added it after noticing restless jerky movements in the syringe motor. It needs a high 95% filter factor to settle down.

The result of the controller, PIDoutput, will be used to adjust the syringe position. Syringe neutral position is added to the result, as we want PIDoutput = 0 to be the state where the submarine is neutrally buoyant and doesn’t move.

#move syringe to the target position
syringeTargetPos = syringeNeutralPos + PIDoutput  #[ml]

The syringe target position is then used to drive the syringe motor in a very simple manner. Just drive the motor at full power until the actual position is equal to the target position.

syringeMotorCtrl = syringeTargetPos - syringePos

#drive syringe motor
motorDive.value = (syringeMotorCtrl > 0)
motorSurface.value = (syringeMotorCtrl < 0)

I added one safety feature to make sure the submarine can always surface and does it as fast as possible. When targetDepth is 0, bypass the PID control and run the syringe to the minimum position.

if targetDepth <= 0:
    #go to surface
    if syringePos > 0:
        syringeMotorCtrl = -100
    else:
        syringeMotorCtrl = 0

PID control for laser distance

Laser PID control is the same, but input data comes from laserDistanceFiltered variable and set point comes in targetDistance variable. As the laser is pointing downwards to the bottom of the pool/lake, the error has opposite sign compared to the depth PID controller.

error = laserDistanceFiltered - targetDistance

To protect the submarine from going too deep, a max depth limit is added.

if depthFiltered >= MAX_DEPTH:
    #limit to max depth
    syringeMotorCtrl = -100

Tuning parameters

I began tests in a 35 cm deep water container. At first I noticed that the water surface tension would hold the submarine and mess up the tests. Very low KP values couldn’t even make the submarine leave the surface. Many times I had to hold the submarine 1 cm below surface at the beginning of tests, to get a clean step response.

I started with changing KP, while keeping KI and KD zero. The tests were step responses from 0 cm to 15 cm targetDepth.

Step response from 0 to 15 cm of depth with different KP factors.

All values made the system unstable. Only the rate and amplitude of oscillation changed.

OK, this is going to be harder than I thought. When starting this project, I really thought PID tuning will be a walk in the park because the submarine is so slow.

I think the problem is with high inertia. The submarine is heavy and responds to commands slowly, as the syringe range is only 42 grams whereas the total weight is 2.4 kg. Once it starts moving, it pushes on like a heavy truck. Need to add derivative to dampen movements and increase stability. Here are test results for KD while KP is 20.

Step response from 0 to 15 cm of depth with different KD factors while KP is 20.

That helped a lot. KD = 200 seems to be the best, as it reduces oscillations but doesn’t slow down the response too much.

Finally I added integrator to fix residual errors. Those may be uncompensated hull compression, error in neutral buoyancy initial setting, sensor read errors, leaks, even changes in atmospheric pressure. Here are test results for KI.

Step response from 0 to 15 cm of depth with different KI factors while KP is 30 and KD is 200.

I want to pick the largest integrator without losing stability. I chose KI = 1. It adds a little bit of overshoot but not too much.

The final tuned parameter set is KP=30, KI=1, KD=200.

Hull compression

While testing the submarine, I noticed a systematic error based on depth. Here is a graph that shows syringe position at 5 cm depth and 20 cm depth.

As you see, the submarine dives too deep when aiming at 20 cm, actually hitting the bottom of the container. It takes over 60 sec for the integrator to fix the error, and when it does, it settles down on using a 1.5 ml lower syringe position. So about 1.5 ml of buoyancy is lost due to hull compression. The same happens when submerging.

To fix it, I added a compensator to the code. The code adjusts the neutral buoyancy position based on depth measurements. The result is used together with PIDoutput to run the syringe motor. The compression effect slows down as depth increases, therefore I used square root of depth in the formula.

#calculate syringe neutral position
#  hull compression compensation (15 ml at 100 cm depth)
syringeNeutralPos = SYRINGE_NEUTRAL_POS
if depthFiltered > 1.0:
    syringeNeutralPos -= 15
elif depthFiltered > 0:
    syringeNeutralPos -= math.sqrt(depthFiltered) * 15 / 1.0

In the formula I used 15 ml compensation at 100 cm depth. It is actually overcompensating, but step responses looked more accurate that way, having less overshoot. In reality the compression effect was 5-10 ml at 100 cm in different test runs.

Here is the previous test again with compensator.

And here another test in a swimming pool with compensation active.

Hysteresis

I have a hysteresis compensation with the syringe position data. I tested it with different settings. The setting is how many rotations the syringe motor needs to do before change in actual syringe position.

The hysteresis compensation seems to improve stability a little. About 1 rotation seems to be the best setting. That is in line with my visual observations, eyeballing the syringe during operation.

Deadband

While testing I noticed how the motor would go crazy with the PID control output. It would change direction constantly and be active all the time. I need some amount of deadband to protect the motor and syringe from wear and to save batteries.

I first tried this very simple limiter. SYRINGE_DEADBAND is the deadband setting in milliliters.

#syringe deadband limitation
if abs(syringeMotorCtrl) < SYRINGE_DEADBAND:
    syringeMotorCtrl = 0

It would nudge the syringe forward at the edge of the deadband range. I found it to limit motor runtime well, but motor activation count was high. Also the syringe would not reach the target position, and therefore the submarine was oscillating more.

Here is the second version I tried.

#syringe deadband limitation
if abs(syringeMotorCtrl) > SYRINGE_DEADBAND:
    deadbandDir = sign(syringeMotorCtrl)
if sign(syringeMotorCtrl) != sign(deadbandDir):
    deadbandDir = 0
if deadbandDir == 0:
    syringeMotorCtrl = 0

It starts the motor when outside of deadband, and stops only after it reaches the target. It does better job in following the target and therefore causes less oscillation. But it sometimes runs the motor back and forth unnecessarily and therefore motor runtime is higher.

I chose the second version, as it leads to better stability.

Here is the syringe position data with different settings.

Here are depth measurements from the same test.

As you see, larger deadband makes the system more unstable. I selected 0.5 ml for the final implementation.

Filters

I have two filters. The first is for measurements, either depth or laser distance. The second is for PID control derivative.

Here is a comparisons of filter settings.

Step response from 0 to 15 cm of depth with different filter settings while KP=30, KI=1 and KD=200. The first percentage is exponential moving average factor for measured depth, the second is for PID controller derivative.

As you see, both too little and too much filtering makes the system unstable. I chose 90% for depth and 95% for derivative (the red line on graph).

Final tests

Here are some tests of the final implementation.

PID over pressure sensor depth measurements. Done in a water container.

PID over laser distance sensors. Done in a water container.

I wouldn’t say the PID control is perfect in any means. There was +/- 10 cm error in depth when testing in a swimming pool. But it was good enough to enjoy driving the submarine around.

RC Submarine 4.0 – electronics (8/10)

2 Replies

Time to add electronic parts and connect wires for the submarine.

Computer

The computer’s main task is to control depth. It takes sensor data and drives the syringe motor in a control loop. The control loop doesn’t have to be fast as the submarine moves very slowly.

I first considered using Raspberry Pi Pico, which is a tiny micro-controller unit with a 133 MHz processor. CPU speed would have been enough, but unfortunately it contains only 2MB of flash memory. That would be too little for the amount of log data I want to store.

So I chose the next one in Raspberry Pi lineup, Raspberry Pi Zero 2 W. It was very difficult to get, because of the worldwide chip shortage, but I luckily found one available. It cost 28 euros, link: https://raspberrypi.dk/en/product/raspberry-pi-zero-2-w/

The board contains a 1GHz quad-core processor and 512MB of RAM, which is more than enough performance for me. I bought a 16GB microSD card to be used as a storage. That will provide enough space for any log files I am going to generate. The board is fairly small (65×30 mm) and will fit nicely into the submarine.

Wireless WLAN connection is also supported. It proved to be a lot more useful than I thought initially. It allowed me to change PID parameters and do fixes to the code without needing to open the lids and connect USB cable to the Raspberry. It made the development process a lot faster. An absolute necessity to have a WLAN.

Since I bought a headerless version, I needed to solder a pin header. I bought a colored pin header, which is much better than a black one. I had used the black header earlier, and it was annoying to count the pins to know where to connect wires.

For attaching and protection, I bought a clear case, which is two plates attached with screws onto the board. It cost 5 euros: https://www.partco.fi/en/raspberry-pi/sbc-housing/22446-rpi-zero-case.html

Raspberry with pin header and a clear case on top.

Then I needed to install the microSD card. Raspberry Pi provides a desktop imager program for it, which is easy to use. I installed Linux-based Raspberry Pi OS operating system onto the card.

A bit more difficult was setting up the communications. I have a laptop computer I want to connect to the Raspberry Pi, using both WLAN and USB cable, just in case the other one dies.

For the USB connection, I had to install Windows RNDIS driver to the laptop to enable Ethernet over USB. Then change a few lines of configurations to the microSD card. Link to the instructions.

For the WLAN connection, I turned the Raspberry into an access point, so that no external access point is needed. I will connect my laptop directly to the Raspberry AP using WLAN. It required a fair bit of configuring. Link to the instructions.

After the connection was up and running, I used VNC Viewer (RealVNC) remote desktop to access it.

Raspberry Pi OS desktop connected through VNC.

I used Thonny Python IDE for running the code. It comes pre-installed with Raspberry Pi OS.

Location

I put the computer near the battery box, where there was most space left.

Two short Lego liftarms and axle pins were enough to hold it in place. No tape was needed.

Wirings

All wires were custom made, as there is little space inside the hull.

The wire thickness I selected is AWG28 or 0.09 mm2. Very thin and flexible, good for tight spaces. Difficult to know how much current it can precisely carry, as there are different numbers in different sources, but at least 1A should go safely. Enough for my needs.

I found this rainbow flat cable to be very good, as it provides many different colors. You just strip away the wire you need. It costs 1.5 euros per meter, link: https://www.partco.fi/en/cables-and-wires/flat-cables/7665-kaa-lat-10-vari.html

For connections I used pin header connectors (DuPond/Harwin connectors), since those are compatible with the Raspberry pin header. I bought a big connector kit that cost 20 euros, link: https://www.partco.fi/en/connectors/pin-header-connectors/pin-header-connectors/19454-liitinlaj-2.html

I also need a crimp tool to attach the connectors to wires. The one I used cost 22 euros, link: https://www.partco.fi/en/tools-and-handtools/crimp-tools/rest-of-crimp-tools/11353-ht213.html

Crimp tool for attaching DuPond connectors.

Crimping is a particular skill. Watching how-to videos helped me to grasp it. Cutting the wire insulation to correct length was important for a clean result, as was to press the crimp hard on difference places for a secure connection.

Here is one finished wire.

Voltage regulators

Lego battery box output voltage is about 8V, but Raspberry takes in 5V. Need a step-down DC/DC converter.

I bought a Pololu 2123 S7V8F5 5V voltage regulator. It cost 20 euros. Link: https://www.partco.fi/en/power-supplies/dcdc-voltage-converters/20031-pololu-2123.html

The chip is very small (11×17 mm), which was my first criteria. Max continuous output current is 1A, which is enough for me as the Lego battery box cannot output more than that. Efficiency is about 90%. The input voltage range is 2.7-11.8V and output is 5V. Everything fits well.

It has four connections: GND, VIN, VOUT, and SHDN. The fourth one is a shutdown pin that we won’t need. So just three pins to connect.

Motor drivers

Next thing is to drive the Lego motors. Raspberry can provide only 3.3V and couldn’t handle the current anyways, so I need motor drivers. The drivers should be able to change speed and direction of rotation. H-bridges can do that when run with PWM signals.

In previous submarines I used L298N motor drivers, but later I found out from different sources that their efficiency is only 40-70%. There are better drivers available. TB6612FNG efficiency is over 90% and DRV8833 is 70-90%.

I had used TB6612FNG before in my Reaction Wheel Inverted Pendulum. Although better efficiency, it requires two connections (VCC and STBY) more than DRV8833. So, I chose DRV8833 for simplicity.

I bought two Pololu 2130 DRV8833 dual motor driver boards. One board can drive two motors. They cost 5 euros a piece. Link: https://www.generationrobots.com/en/401023-drv8833-dual-motor-driver-pololu.html

This chip is also very small (13×20 mm), great for a small submarine. Voltage range is 2.7‌‌-10.8V, good for a Lego battery box that outputs 8V. Max continuous output current per motor is 1.2A, well above my needs.

This one has many connections. GND and VIN are connected to battery box. AIN1 and AIN2 are connected to Raspberry GPIO pins to provide PWM signals for speed and direction. AOUT1 and AOUT2 are connected to the motor. And BIN and BOUT for another motor.

I attached these boards on the Raspberry case using double-sided tape and connected the wires.

Voltage regulator and two motor driver boards on top of Raspberry Pi Zero 2.

Everything is connected

Besides the voltage regulator and motor drivers, there were other connections. Pressure sensor was connected to the Raspberry I2C bus. Laser distance sensor was connected to the Raspberry serial UART line. Syringe motor tacho outputs were connected to Raspberry GPIO. Radio inputs and power output were connected to Raspberry GPIO pins.

It is a complex mess. Here are few images of the final wiring.

Power consumption

Here is the current draw for each part, either from specs or measured by me:

150 mA @5V Raspberry Pi Zero 2
90 mA @5V laser distance sensor
6 mA @3.3V tacho power
2 mA @3.3V radio receiver
0.4 mA @3.3V pressure sensor
0.1 mA @3.3V motor drivers

All power goes through the 5V voltage regulator, which has 90% efficiency. The Raspberry’s internal regulator is also used, but for so little current, that I’m leaving it out.

The total power consumption is 1360 mW.

  P = (150*5 + 90*5 + 6*3.3 + 2*3.3 + 0.4*3.3 + 0.1*3.3) / 0.9
    = 1360 mW

The battery box output voltage is about 8V. Therefore the total current draw from battery box is 170 mA.

  I = P / U
    = 1360 / 8
    = 170 mA

That is the minimum current draw. If motors are used, it will be higher.

I measured the current draw in different scenarios while the submarine was on a table:

170-190 mA submarine idle
300 mA main propeller turning
250 mA turn propeller turning
350 mA syringe moving
550 mA syringe moving + main propeller turning + turn propeller turning

Those measurements were done in air. Propellers turn slower in water and draw more current, so a little bit extra need to be added. I estimate the maximum peak current draw to be about 700 mA when all motors are running.

Battery box current limit

The battery I’m using is a Lego rechargeable battery box, part id 8878. It contains a current limiter with 750 mA theoretical value, but based on Philo’s measurements, it can provide even 1.4A for 10 seconds. Link: https://www.philohome.com/batteries/bat.htm

To be sure, I tested the unit myself. In my tests, it supplied 1.0A for more than 60 seconds (I didn’t test longer than that). When drawing 1.2A, the current limiter kicked in after 10 seconds.

Okay, the limit is about 1A. That is above the 0.7A peak current draw I’ve estimated. It should work just fine.

Heat

Since the submarine is a small enclosure, it should heat up. The electronic parts consume 1.4 Watts of power at idle. The energy will dissipate first into the air and later through the hull walls into the surrounding water. How well the “water cooling” effect works is unknown to me.

Heating could lead to problems. The first potential problem is the battery box current limiter. According to Philo the limiter is a Bourns MF-MSMF075 resettable fuse, which also acts as an overtemperature limiter. The specs says that the hold current is 750 mA at 23 degrees Celsius, and it will drop to 430 mA at 85 °C. That could be enough to trip it when all the motors are driven. The result would be a sudden power loss in my submarine.

The second problem is the Lego plastic melting. The melting point is 105 °C, but Lego starts to soften already at 60 °C.

Raspberry max temperature is 85 °C. It will start to reduce CPU speed at 60 °C.

All in all, 60 °C seems to be the limit under which I can safely operate.

I have three temperature sensors inside the submarine. One is in the Raspberry unit measuring CPU temperature, and the other ones are in the laser distance sensor and pressure sensor.

Here is temperature data from a 26 minute test run through a small river in a hot 30 °C summer day in Finland. Water temperature is about 25 °C. The river is partially shadowed by surrounding trees.

As you see, the CPU temperature does increase slowly from 40C to 48C and the laser sensor follows the same ascending path. Pressure sensor, on the other hand, stays the same. I assume the difference is from power consumption. CPU and laser are very hungry on energy, but pressure sensor is not. The pressure sensor probably follows most closely to the ambient temperature. Interestingly the bumps at 23 minutes are from a sunny section of the river. The pressure sensor reacts fast to the increased temperature, which comes either from the water or from the sun pointing to the black sensor unit.

In any case, the temperatures are well under 60 °C. No problems with heating.

Battery run time

The Lego rechargeable battery box contains a 1100mAh LiPo. If average current flow is 250 mA, the battery should drain in 4.4 hours.

The actual time I’ve experienced has been about 2 hours. It is enough.

WLAN range through water

WLAN works in 2.4GHz frequency, which shouldn’t penetrate water well. After using it extensively, it seems to be able to pass through about 10-20 cm of water. Just enough to communicate when the submarine is at the surface.

That helps a lot, since you can adjust settings and start/stop the software without lifting the submarine out of water.

Even better, when I tested it in a large water container (46x26x27 cm), the WLAN connection was never halted. I could watch the console readings through VNC connection in real time. That helped to debug problems.

In a swimming pool, 10 cm of depth seemed to be the limit.

RC Submarine 4.0 – radio (7/10)

9 Replies

Submarine needs a low-frequency radio controller that is able to penetrate water. At least 27 MHz, 40 MHz and 75 MHz bands should work.

Those are hard to find. Nearly all available radio controllers use 2.4 GHz. I’ve resorted to buying cheap toy submarines that use proper radio frequencies, and cannibalize them for the receiver board and use their transmitter. It is a sub-optimal solution, but it has worked well enough in my three previous submarines.

Once gain, I continued the same path, using the same no-name Chinese toy manufacturer as before.

A toy submarine

I bought a mini u-boat toy submarine of a chinese brand Mliu. It cost 33 euros in Amazon store TAIPPAN, link:
https://www.amazon.de/-/en/Control-Military-Electric-Rechargeable-Swimming/dp/B08T66M8P7/

The main criteria in buying this particular toy, besides the fact it uses 27 MHz radio frequency, was the nice looking transmitter this has. Very toyish, but it still appeals to my eye.

The transmitter has 6 buttons for control: left, right, forward, backward, dive, surface.

After I tested that the toy submarine works, I disassembled it. I cut all wires leading to motors and took out the single board it contains.

Here is the board that came out.

Here is a closeup of the board from both sides, after I removed the big green cylinder that was a LiPo battery.

External power supply

In previous submarines I’ve supplied the radio board with a LiPo battery. Now I wanted to remove that and use power from the Lego battery box. It would make recharging the submarine easier, as there would be only one battery on-board.

But does the radio board work with voltage other than 3.7V it it designed for?

I tested it using an adjustable power supply, and found the minimum voltage that made the board function to be 2.3V. That means I can use the Raspberry Pi 3.3V output to power up the radio board. Great!

I also verified that the control range was not affected. It was around 7 meters regardless of supply voltage.

Connection to Raspberry

First I checked if I need pull up/down resistors to protect the Raspberry. Raspberry GPIO pins can take 0-3.3V. I measured voltage from the radio board motor wires that I’m using to read button presses. The voltage was either 0V or 3.3V. Great, I can connect them directly to Raspberry.

Then I connected the wires to Raspberry GPIO pins. 6 wires for buttons, one ground wire and one power wire.

Radio board wires. The blue wire on the left is the antenna. I added a DuPond connector to it for testing different antennas.

Code

I first created Button instances for all the GPIO inputs and one output for powering the radio.

#prepare radio control
radioPower = DigitalOutputDevice(25)
buttonForward = Button(24, hold_time=1.0)
buttonBackward = Button(23, hold_time=1.0)
buttonLeft = Button(9)
buttonRight = Button(10)
buttonDive = Button(11, bounce_time=None, hold_time=1.0, hold_repeat=True)
buttonSurface = Button(8, bounce_time=None, hold_time=1.0, hold_repeat=True)

tachoPower.value = 1

Then for left and right buttons I added handlers that write values to the motor output. Here is a snippet of the relevant parts.

motorLeft = DigitalOutputDevice(17)

def buttonLeft_pressed():
    motorLeft.value = 1
def buttonLeft_released():
    motorLeft.value = 0

buttonLeft.when_pressed      = buttonLeft_pressed
buttonLeft.when_released     = buttonLeft_released

For forward and backward buttons I added a little crawl period for the first second, in which PWM duty cycle is only 60%. It helps maneuvering in tight places.

motorForward = PWMOutputDevice(22)
motorForward.frequency = 1000

def buttonForward_pressed():
    motorForward.value = 0.6
def buttonForward_held():
    motorForward.value = 1
def buttonForward_released():
    motorForward.value = 0

buttonForward.when_pressed   = buttonForward_pressed
buttonForward.when_held      = buttonForward_held
buttonForward.when_released  = buttonForward_released

Surface and dive buttons don’t control motors directly, instead they change target depth or target distance for the PID controller.

def changeTargetDepth(value):
    global controlMode
    global targetDepth
    global targetDistance
    if controlMode == MODE_PID:
        targetDepth += value
        if targetDepth < 0:
            targetDepth = 0
        if targetDepth > MAX_DEPTH:
            targetDepth = MAX_DEPTH
    elif controlMode == MODE_PID_LASER:
        targetDistance -= value
        if targetDistance < 0.1:
            targetDistance = 0.1
        if targetDistance > MAX_DISTANCE:
            targetDistance = MAX_DISTANCE

def buttonSurface_pressed():
    changeTargetDepth(-0.05)
def buttonSurface_held():
    changeTargetDepth(-0.5)

buttonSurface.when_pressed   = buttonSurface_pressed
buttonSurface.when_held      = buttonSurface_held

Change control mode

I need to change between modes. The PID can be controlled using a pressure sensor or a laser distance sensor, so I need to change between those two. Also, I need to be able to bypass PID control and drive syringe directly, e.g. to empty the syringe after dive.

But the radio transmitter has only 6 buttons which all have a function already. The only way I can think of is to use button sequences. Three button presses in a certain order in 1.5 seconds to change the mode.

#change depth control mode
#  triggered by button sequences
#    MODE_DIRECT    = Surface-Dive-Surface
#    MODE_PID       = Surface-Dive-Right
#    MODE_PID_LASER = Surface-Dive-Left
changeMode = False
if buttonSurface.value == 1:
    if modeButtonStep == 0:
        prevModeButtonTime = perf_counter_ns()
        modeButtonStep = 1
    if modeButtonStep == 2:
        modeButtonStep = 3
        controlMode = MODE_DIRECT
if buttonDive.value == 1:
    if modeButtonStep == 1:
        modeButtonStep = 2
if buttonRight.value == 1:
    if modeButtonStep == 2:
        modeButtonStep = 3
        controlMode = MODE_PID
if buttonLeft.value == 1:
    if modeButtonStep == 2:
        modeButtonStep = 3
        controlMode = MODE_PID_LASER
if modeButtonStep != 0 and (perf_counter_ns() - prevModeButtonTime) / 1e9 >= 1.5:
    modeButtonStep = 0

Problem with auto-surface

While testing the board, I kept having problems with the surface button. The surface motor wire would turn on apparently by itself and the surface button stopped working. What was going on?

It took me awhile to figure it out. The manual calls it “automatic lifting function”. When no buttons are pressed within 10 seconds, the submarine will automatically rise to the surface. This is a new feature in this brand of toy submarine, which is why I hadn’t noticed it in previous submarine projects.

I spent a lot of time trying to bypass it. First I reverse engineered the operation principle. The little toy submarine has a piston ballast, like I’m making here. 🙂 The ballast has limit switches to indicate when to stop driving the motor. The auto-surface feature is waiting for the ballast to reach the limit, and then it will stop driving the motor. I connected the switch wire to Raspberry and put code in to trigger it every 5 seconds, to make the board think it is at surface. It worked most of the time, but sometimes the surface button still stopped responding. What made it worse, if the auto-surface thing was turned on only for a moment, you would need to press dive button once to reset it. Otherwise the surface button was dead.

Eventually, I came up with a brutal idea. I just reset the whole board. Since I’m powering it through Raspberry GPIO pin, I can control when to power it.

I put a code to reset the board very quickly every 9 seconds. Or more precisely, after 9 seconds of not pressing any buttons, since I can detect all button presses in code. The power is cut off for one loop interval, 25 ms, which is long enough to reset it.

#radio controller fix
#  the radio board has an auto-surface safety feature
#  after 10 secs of silence it will make the surface button to not work
#  to fix it, we will quickly power off and on the radio every 9 secs
if (buttonDive.value == 1 or buttonForward.value == 1 or
   buttonBackward.value == 1 or buttonLeft.value == 1 or buttonRight.value == 1):
    prevRadioResetTime = perf_counter_ns()
secsSinceRadioReset = (perf_counter_ns() - prevRadioResetTime) / 1e9
if secsSinceRadioReset > 9:
    radioPower.value = 0
    prevRadioResetTime = perf_counter_ns()
else:
    radioPower.value = 1

That took care of it – the radio worked always after that.

Problems with range

When I got to test the radio inside the hull, with Raspberry and all other parts running, I noticed the range was only 3 meters. That was odd. I had tested it before disassembly, and the range should be 7 meters.

Maybe extending the antenna will help? In my earlier submarines I added a longer antenna and increased the range from 7 to 10 meters. I tried it, but this time it had no effect.

Maybe the Raspberry’s noisy supply voltage causes frequency interference? After all, the voltage from Lego rechargeable battery box, that is initially clean, coming from two LiPos, goes first to a 5V switching regulator, and then to Raspberry’s internal 3.3V regulator, before reaching the radio board. So it probably has a lot of noise.

I tried to supply the radio from a clean power supply, and that seemed to fix the problem at first. Range was increased to 7 meters. But when I added a separate LiPo for the radio board, the range was dropped back to 3 meters. I was getting mixed test results.

Okay, maybe the radio picks up electromagnetic interference from wires and other parts inside the hull? After all, it is located on top of the Raspberry, very close to all other electronic parts.

The first location was on top of the Raspberry.

I tried a few different locations. If the receiver was 20 cm away from other parts, range was 7 meters, but dropped to 3 meters when close to other parts. Okay, I’ll locate the radio board as far as possible inside the hull, about 10 cm away.

I moved the location to the back, on top of the main propeller motor.

That increased the range to 4 meters. Still quite bad.

Once again I tried different antennas. 20 cm long straight antenna didn’t improve the range but a big looping antenna along the hull cylinder increased range to 6 meters at first. But when I tested it again in swimming pool, it had no effect, range was only 3-4 meters, which is the same range I got with a short 3 cm antenna.

At this point, I was having a lot of mixed information and test results. This radio interference feels like black magic. Probably all the things I tried have some effect, I just can’t measure it in any reliable way.

I gave up trying to fix it. The radio control range is only 3-4 meters, just deal with it. It was enough to do the tests and videos I needed to do, including tests in a swimming pool and through a very small river. But it was annoying, sometimes having to press buttons multiple times to get through.

Testing the radio control in a swimming pool.

RC Submarine 4.0 – laser distance sensor (6/10)

The first laser sensor

The first laser distance sensor I bought was Whadda WPSE337. It uses the STMicroelectronics VL53L0X Time-of-Flight module. It cost 18 euros in a Finnish retail shop, link: https://www.radioduo.fi/p/p/WPSE337/.

The sensor is very small and it communicates through I2C. Range is 30-2000 mm. The purpose is to keep the submarine in about 20 cm distance to the bottom, so 2 meters would be enough. Sampling time is 30 ms, that means about 33 samples/s, which is not great but maybe enough for PID control. Accuracy is +/- 3 % which would be 0.6 cm at 20 cm distance. That would be enough.

I installed the sensor against the bottom of the hull using Lego parts and a small rubber band.

Laser distance sensor pressed against the bottom of the hull. The hull is upside down in this picture.

The sensor has four wires: GND, Vin (3.3V), SDA, SCL. After connecting the wires to Raspberry, I downloaded a Python library for the sensor: https://github.com/johnbryanmoore/VL53L0X_rasp_python

I wrote a simple program to test the sensor. I selected “high accuracy” ranging profile that has a timing budget of 200 ms. Light travels slower in water, so compensation was needed.

I tested the sensor in a jar filled with water. The hull was pressed against the water surface so that there was no air in between plastic and water. I lowered a Lego plate in to the water and started moving it up and down to try different distances.

At first the data looked accurate. Distance was correct when compared to the measuring tape next to the container. When the plate was pressed against the hull, having less than 1 cm distance to the sensor, it gave 2-3 cm readings, which was too much. But that was expected as the range minimum is 3 cm. But then when I lowered the plate to 15 cm, I noticed the readings stayed at 12 cm.

I tried again with a 35 cm deep water container to verify the problem: max distance is 12 centimeters. Disappointing. What was worse, the sensor didn’t stop giving data, but instead it gave wrong data. It gave random values between 6-12 cm when the actual distance was more than 12 cm. That basically made all data is unusable.

Wavelength affects water absorption

Why is 2 meter range limited to 12 cm underwater? That baffled me. The water I tested against was crystal clear and I could see through it easily. Then I got an idea. The laser uses an infrared wavelength of 940 nm. Our eyes see a range of 400-700 nm. That is the only difference I could think of.

I looked up water absorption spectrum curves online. The absorption coefficient does climb very steeply after leaving the visible spectrum at 700 nm. It is about 0.6 at 700 nm and increases to 40 at 940 nm (unit 1/m). So almost 100 times increase in absorption. No wonder the sensor range is limited. Below are the curves I looked at. Note that both have logarithmic scale on y-axis.

Based on the curves, visible light 400-700 nm or ultraviolet light between 300-400 nm would be best for underwater measurements. Unfortunately, I couldn’t find any sensors in that range. But I found one sensor that uses 850 nm wavelength. The absorption coefficient for that wavelength is about 5. That is almost 10 times better than the first sensor. Let’s try that.

The second laser sensor

The second laser distance sensor I bought was Sparkfun TFMini-S Micro. It cost 55 euros, link: https://www.elfadistrelec.fi/fi/p/30216198.

As mentioned, this laser has a wavelength of 850 nm, which should provide 10 times less absorption compared to the previous sensor. Range is max 12 meters, a hefty increase from the previous sensor. Range minimum is 10 cm, which might be a problem, as the plan is to keep submarine distance to 10 or 20 cm from bottom. Frame rate is stated to be 100 samples/s in this datasheet table, but it actually can go up to 1000 samples/s, so improvement there. Accuracy is 6 cm, a little bit lower than the previous sensor. Have to see how that works with PID control.

Power consumption has increased considerably. The previous sensor consumed 10 mA but this sensor can draw up to 140 mA in average based on the specs. Actually, I measured the current flow myself, and it is 93 mA when pointing the sensor at a far away object or black paper or something other difficult, and drops to 50 mA when measuring close distance. It seems to have those two states it switches between automatically. I didn’t find any commands that would limit the power consumption, even lowering frame rate has no effect, except for 1 Hz frame rate that puts it into a 13 mA low power mode.

High current might bring problems with the Lego battery box. It can output max 1A, and it has many other components to supply. I have to keep an eye on that. Also, the 3v3 output pin on Raspberry Pi Zero 2 is rated for only 50 mA, but luckily this sensor uses 5V input, so I can power it directly from a 5V regulator. The regulator is rated for 1A.

Field-of-view is only 2.2 degrees, much less from 25 degrees in the first sensor. That brings me to another potential problem. This sensor is fairly big compared to the first one, and the emitter and receiver are 10 mm apart. If you lay the sensor sideways to the direction of the hull, the curvature of the hull will cause refraction when laser beam moves from air to plastic and from plastic to water. Actually, I did some quick calculations with the refraction formula and, if I’m correct, a laser beam that leaves the emitter straight will come back to the receiver at a 3.5 degree angle. That would be too much for the 2.2 degree FOV. Have to test that.

The sensor has four wires: GND, 5V, RXD/SDA and TXD/SCL. The package included two wires to connect the strange plug this sensor has. The first thing I had to do is cut one of the wires and solder female DuPont connectors to it. Then I could connect it to Raspberry.

The sensor supports both I2C and UART Serial communication. I first tried to use I2C, but the sensor stayed silent. Looks like UART mode is on by default and you need UART to change the mode. Luckily I have UART pins on Raspberry. I connected those and got a response.

I read the specs and looked up examples online to figure out how to communicate with the sensor. Pretty simple otherwise, but the command checksums were annoying to calculate by hand. Output data includes distance, signal strength and temperature.

from serial import Serial

#prepare laser distance sensor
serial = Serial("/dev/serial0", 115200, timeout=0.01)
serial.write([0x5A,0x05,0x05,0x06,0x6A])  #set unit to mm
serial.write([0x5A,0x06,0x03,0x64,0x00,0x39])  #set frame rate to 100 Hz
serial.write([0x5A,0x09,0x3B,0x00,0x00,0x00,0x00,0x00,0x62])  #set I/O mode to standard
sleep(0.1)
serial.reset_input_buffer()

def readLaserSensor():
    #skip old serial data
    FRAME_SIZE = 9
    while (serial.inWaiting() > FRAME_SIZE):
        serial.read()

    #read data
    data = serial.read(FRAME_SIZE)
    
    #parse data
    distance = float('nan')
    strength = float('nan')
    temperature = float('nan')
    if len(data) == FRAME_SIZE and data[0] == 0x59 and data[1] == 0x59:
        checksum = 0
        for i in range(0, FRAME_SIZE - 1):
            checksum = checksum + data[i]
        checksum = checksum & 0xFF
        if checksum == data[FRAME_SIZE - 1]:
            distance = data[2] | data[3] << 8  #[mm]
            strength = data[4] | data[5] << 8  #[0-65536]
            temperature = data[6] | data[7] << 8
            
            distance = distance / 1000   #[m]
            temperature = temperature / 8 - 256  #[C]
    
    return distance, strength, temperature

Here is the adjustment for water.

#scale for water
SPEED_IN_AIR = 299000  #[km/s]
SPEED_IN_WATER = 225000  #[km/s]
distance *= SPEED_IN_WATER / SPEED_IN_AIR

While testing close distances through water, I noticed a systematic error: 18 cm is actually 20 cm, 10=15, 5=10, 2=5, 0=0. So I added a little compensation formula for close distances.

#fix laser data for close distance (when sensor says 0.05m it is actually 0.10m)
laserDistanceFixed = laserDistance
if laserDistanceFixed < 0.25:
    laserDistanceFixed += 0.5 * (0.25 - laserDistanceFixed) * \
                          math.sqrt(laserDistanceFixed / 0.25)

Later when I tested the submarine in nature, I started getting a lot of unreliable data. Of course, a muddy river floor will not reflect the laser well. The signal strength seemed to be below 300 when the data was very noisy.

The signal strength is a number between 0-65535, higher better. The spec says lower than 100 is unreliable and the sensor will send -1 as distance. Well, the value I received was 49.316 meters, don’t know where that comes from, and it may come even when strength is a little bit larger than 100. Whatever, I just use both strength and distance to omit bad values. In case of bad data, I will pass old data to the PID controller for max 1 second. After 1 sec I will give NaN to indicate no distance can be found.

#omit unreliable laser data
if laserStrength < 300 or laserDistance > 10:
    laserDistanceFixed = float('nan')
    if (perf_counter_ns() - laserDistanceValidTime) / 1e9 >= 1.0:
        laserDistanceFiltered = float('nan')
else:
    if math.isnan(laserDistanceFiltered):
        laserDistanceFiltered = laserDistanceFixed
    laserDistanceValidTime = perf_counter_ns()

Lastly a filter. I chose 0.95 ratio instead of 0.90 as the laser is noisier than the pressure sensor.

#filter laser distance (exponential moving average)
if not math.isnan(laserDistanceFixed):
    laserDistanceFiltered = 0.05 * laserDistanceFixed + 0.95 * laserDistanceFiltered

I tested it in a water container, similar way to the first sensor, using a red Lego plate to change distance.

Tests were done for both sideways orientation and lengthwise orientation. The Lego plate was measured at four distances: 33 cm, 20 cm, 10 cm, and 1 cm. As you see in the graphs below, sideways orientation does give correct readings but the data is noisier and signal strength is below 2000. In lengthwise orientation strength is much higher between 1000 and 20000. Interestingly, strength is higher in 33 cm distance than in 20 cm distance, no idea why, maybe the sensor switched to high power mode. No filter was used in these tests, the data is raw, only corrected for water and close distances.

So, refraction through the curved hull does have an effect. Less than I expected based on my calculations, but still. The sensor orientation needs to be lengthwise. I first thought that would cause a problem pushing the sensor too far to the tungsten pellet area, but it was fine. I made a little support construct from Lego parts to hold the sensor still.

The hull is upside down in this picture.

Later I tested the submarine in a swimming pool that is 1.2 meters deep and the floor is painted with bluish white/gray paint that reflects light fairly well. Water is clear with visibility to maybe 10 meters. Below is a graph that shows pressure sensor and laser data on top of each other. The swimming pool floor is even with only slight tilt, therefore depth and distance to bottom should be relative to each other. From that graph we can see that the maximum distance is 50 cm. Above that the graph doesn’t show lines, as the measured value is NaN.

And here are tests in a small river, about 30 cm deep, and it has a brown muddy bottom with branches and rocks laying around. Water is dirty with visibility to maybe 3 meters. As you see, the data is very patchy and not really usable for control, even though distances are quite small.

In the end, I didn’t use the laser much while playing with the submarine. The data was too unreliable for that.

RC Submarine 4.0 – pressure sensor (5/10)

6 Replies

Need to measure the depth of a submarine. Let’s read some datasheets.

The first pressure sensor

The first pressure sensor I bought was Honeywell ABP2LANG004BG2A3XX. It cost 26 euros in a Finnish retail shop, link: https://www.radioduo.fi/p/p/ABP2LANG004BG2A3XX/

It is very small (8x6x10mm), which is great as there is always too little space inside the hull. It has a barbed port that is easy to connect to a hose, perfect!

The communication output is I2C, which is great since I’ve used that before. Besides I2C there were also SPI and analog sensors available, but SPI requires 4 wires instead of 2, and I can’t read analog with Raspberry Pi Zero 2, so that was an easy decision. The spec says this sensor is suited for liquid media, very convenient. Pressure range is 0…4 bar, which is 0…40 meters in water depth. I would have preferred less than that, since I plan to dive max 4 meters and larger range means less accuracy, but fine.

Data rate is 204 samples/s, which is more than enough. From previous subs I know they move very slowly. Water resistance dampens all movements and the subs are heavy compared to the control force, which makes them behave like heavy trucks or something. It could take 10 seconds to submerge from 1.5 m depth. So timing shouldn’t be an issue in the PID control. But high data rate is good as it allows me to use more filtering and further improve accuracy.

But accuracy numbers look disappointing. Datasheet says ±0.25 %FSS BFSL. FSS is Full Scale Span which in our case is 4 bar. BFSL is Best Fit Straight Line which is a kind of an average of the measurements. So 0.25% of 4 bar is 10 mbar which equals about 10 cm of depth. That is bad. I would want the sensor accuracy to be about 1 mm, or at worst case 1 cm. But that is the best I found, so I have to deal with it.

After I received the sensor, I had to solder wires to the pads. Why did I buy a Leadless SMT version, as there were other packages available? Those pads are almost too small to be soldered by hand. I could break the sensor by heating it too much or make a bad solder connection that breaks during dive. A first of many bad decisions.

I connected the wires to Raspberry to test it. Four wires: GND, Vdd (3.3V), SDA, SCL. Then I wrote a simple Python code to read data from the sensor.

The first test I did was against a Lego barometer. I used three hoses and a T-splitter to connect together the sensor, the barometer and a Lego pneumatic pump. I used the pump to increase the pressure to 2 bar. This was just to verify that I wrote the Python code correctly. Sensor readings matched that of the barometer. Great!

Then I measured the amount of noise the data has. I kept the sensor on a table, read data 100 samples/s, and calculated standard deviation. The result was 0.00017 bar which is 1.7 mm of depth. So basically 99.7% of the samples were in a 1 cm range. That was surprisingly good. Based on the datasheet I expected worse. Then again, those datasheet numbers are from the entire pressure range and probably very conservative anyways.

One odd thing I noticed that there were spikes in the data. Randomly one sample would be 10-50 cm too large. It happened once in a while, sometimes every second or so. I never knew why. I just put a few lines of code to filter out outliers and moved on.

Then I made the first test with water. I connected a hose to the sensor and lowered the end of the hose into a container filled with water. This isn’t the best way to test a sensor since the air inside the hose will compress and make the depth readings too small, but it is accurate enough for this purpose. I was interested to see the amount of noise in water pressure readings, and how it fluctuates when you move the hose. I was happy with the results. Noise was in a 1 cm range, response to hose movements was very quick, and the readings were accurate when compared to a measurement tape at the side of the container.

Problem with relative pressure values

At this point I will move much further into the building process, to a time where I first tested the submarine in a big water container. That is when I noticed an unsolvable problem with this sensor. What I had in my hands was a relative pressure sensor. That means it measures pressure relative to the surrounding ambient pressure. Ambient pressure in our case is the pressure inside the hull. I first noticed this when I closed the hull lids airtight and pressed buttons to move the syringe ballast. Sensor readings started immediately to change, even when the submarine wasn’t moving. Damn it!

I did some calculations to verify the problem. The free air space inside the hull is about 2000 ml and the syringe active capacity is 40 ml. Therefore, when the syringe goes from fully retracted to fully extended, the air space inside the hull will diminish from 2000 ml to 1960 ml. I put the numbers into an online Boyle’s law calculator and got a result of pressure increase from 1 bar to 1.0204 bar. That means about 20 cm increase in depth, which was roughly the effect I saw in my tests. So I knew the cause, but this wasn’t good. That error is too large and it will mess up the PID control.

But maybe I can compensate for it in code? I know the syringe position from tachometer data. I will just use the Boyle’s law equations to fix the pressure reading after I’ve read it. So I did put a few lines of code, and the measurement data started to look good again.

Still not good. I did more tests in the water container and noticed moments when the sensor data was a few centimeters off. Not as big error as before, but still large enough to cause problems. This time the problem was with moving end caps. I had had problem with pushing the end caps to their innermost position, because the air inside the hull pushes back. So the caps had some room for movement. They moved because of the outside water pressure and the inside hull pressure, depending on the submarine depth and the syringe position. You couldn’t estimate these movements because of the static friction on the o-rings and such. Later I did find a solution the push the lids fully inside, but at this moment I saw this as another source of unreliability to the sensor data.

For a brief moment I thought about adding another sensor to measure the pressure inside the hull to provide full and accurate compensation. But that would mean more wires and more complexity that shouldn’t be necessary in the first place. So I decided buy a completely new sensor to replace the first one.

The second pressure sensor

The second pressure sensor I bought was Honeywell SSCMANV030PA2A3. It cost 35 euros in Elfa Distrelect, link https://www.elfadistrelec.fi/fi/p/30159454.

This is an absolute pressure sensor, meaning it measures pressure relative to vacuum. The readings are not affected by surrounding ambient pressure.

In some ways this is identical to the first sensor, as it has a barbed port and it is suitable to liquid media. But there are also some differences. This is slightly larger (7×13.3×13.7mm). The pressure range is 0 to 30 psi (2.06 bar), but as this is an absolute sensor, the range is max 1 bar relative to atmosphere. So it can measure up to 10 meters of depth, which is enough for my submarine. Sample rate has gone up to 1000 samples/s (1 ms response time). Accuracy has been doubled from 10 cm to 5 cm, as the range is 2 bar instead of 4 bar. That is good news, but at the same time, output data resolution is only 12 bits (0.03%) whereas the first sensor had 14 bit output. That is slightly concerning, as 12 bits is only 0.5 cm of depth. Have to see how it fares.

Also, I was careful to pick an SMT (Surface Mount Technology) package to make the soldering easier.

Here is the sensor after soldering, heat-shrink tubes installed and silicone hose attached to the port.

When I tested this sensor, I found no random spikes in the data, as with the first sensor. I was happy to remove those ugly outlier filters and simplify the code. The syringe position compensation code was also removed.

Here is the Python code I used to read the sensor using I2C communication. Note that the sensor returns both pressure and temperature. I don’t need temperature, but I read it just to put it into log file, in case I run into overheating problems that I need to investigate.

import smbus

bus = smbus.SMBus(1)

def readPressureSensor():
    #read data
    PRESSURE_SENSOR_ADDR = 0x28
    data = bus.read_i2c_block_data(PRESSURE_SENSOR_ADDR, 0x00, 4)
    status = (data[0] & 0xC0) >> 6
    pressCounts = data[1] | ((data[0] & 0x3F) << 8)
    tempCounts = ((data[3] & 0xE0) >> 5) | (data[2] << 3)
    
    #pressure conversion
    P_MAX = 2  #[bar]
    P_MIN = 0  #[bar]
    O_MAX = 0.9 * pow(2,14)
    O_MIN = 0.1 * pow(2,14)
    pressure = (pressCounts - O_MIN) * (P_MAX - P_MIN) / (O_MAX - O_MIN) + P_MIN  #[bar]
    
    #temperature conversion
    T_MAX = 150  #[Celsius]
    T_MIN = -50  #[Celsius]
    T_COUNTS = pow(2,11) - 1
    temperature = tempCounts * (T_MAX - T_MIN) / T_COUNTS + T_MIN  #[Celsius]
    
    return pressure, temperature

Here is the conversion from pressure to depth. 1 bar equals about 10 meters of depth in water.

#calculate depth
GRAVITY = 9.80665  #[m/s2]
WATER_DENSITY = 998  #fresh water at 20 Celsius [kg/m3]
pressurePa = pressure * 100000  #[Pa]
depth = pressurePa / (GRAVITY * WATER_DENSITY)  #[m]

I also need a reference pressure. One Finnish weather station website says the atmospheric pressure fluctuated from 0.974 to 1.033 bar in year 2021. That means 59 cm of change in depth. Wow, that is a lot! You could probably get several centimeters of error during a single day. So the reference cannot be hardcoded into the code, it needs to update every time I startup the software. I added some code to take the first reading from the pressure sensor and use that as a reference.

Also note that the pressure sensor is not located exactly at the surface. It is about 7 cm below the surface. The reference point has to account for that also, but to do it, I’d have to start the software while the submarine is floating in the water. So, to make the usage a little bit easier, I added some code to adjust the reference point slowly when it is floating at the surface. I used syringe position = 0 as a proof of surface position, as I really don’t have any better way to know that. In the end, I started the software through WLAN while the submarine was floating in water, so the adjustment code was almost never needed.

#adjust for depth zero point
if math.isnan(depthZeroPoint): depthZeroPoint = depth
if syringePos <= 0:
    #when syringe is empty, assume we are in surface and start adjusting
    depthZeroPoint = 0.01 * depth + 0.99 * depthZeroPoint
depth -= depthZeroPoint

I added also some filtering to the depth data. I chose exponential moving average, because it is very easy to implement in a single line of code. The main reason was to help the Derivative part of PID control, which is really sensitive to noise.

#filter depth (exponential moving average)
depthFiltered = 0.1 * depth + 0.9 * depthFiltered

Here is an example of the sensor data from a dive in a small river. Both unfiltered and filtered depth is shown. As you see, the 12-bit resolution makes the raw data a bit jerky, but filtering averages that out nicely without introducing much delay.

This sensor proved to be a good one. Data has been accurate, no spikes and no errors from the syringe position or the end cap movements. I’ve used it 10+ hours underwater without any hiccups.

RC Submarine 4.0 – propellers (4/10)

5 Replies

I have used different types of Lego propellers in my previous submarines. None of them are very good.

Here are three of of those Lego props and two non-Lego props. I tested thrust for them using two Lego PF L-motors, a Lego battery box, a weight scale and a bucket filled with water.

Here are the results. Each prop was tested multiple times with different gear ratios. I’ll list here the ratio that gives the most thrust. Thrust is in grams.

Orange Lego 3-blade (6041) – 45 grams using 1:25 gear ratio
Double grey Lego 2-blade (2952) – 130 grams using 1:5 gear ratio
Black Lego 3-blade with gear (2740c01) – 200 grams fwd, 50 grams bwd using 1:3 gear ratio
Non-Lego boat prop 2-blade P1.4x40mm 83 serien – 75 grams fwd, 40 grams bwd using 1:15 gear ratio
Non-Lego drone prop Diatone Bull Nose Plastic 4 x 4.5 – 250 grams fwd, 100 grams bwd using 1:5 gear ratio

The big black Lego propeller (2740c01) is quite good. I used it in Submarine 3.0. But you need to wrap an ugly rubber band to rotate it, because it has no axle hole but only a gear with permanent connection to the propeller.

Therefore I decided to go for the drone propeller. This will be the first time I use a non-Lego prop.

Main propeller

For the main propeller I selected Diatone Bull Nose with size 4 x 4.5. It is designed to be used in drones. A bag of 4 pieces cost only 1 euro in HobbyKing, link: https://hobbyking.com/en_us/diatone-bull-nose-plastic-propellers-4-x-4-5-cw-ccw-black-2-pairs.html

I selected this particular drone propeller, because the length is 10.2 cm (4 inches) and therefore will match the 11 cm diameter hull of my submarine. Second criteria was to pick the largest pitch I could find, so that it works with low RPM and I don’t need to add many gears. This has 11.4 cm pitch (4.5 inches). I didn’t test any other drone propellers, so I don’t know if this was the best choice.

The propeller has a 5 mm hole in the center, almost perfect for a 4.8 mm Lego axle. But how to rotate it, since there is nothing to hold on to? Normally they would be screwed tightly to the drone rotors and let friction keep them in place. But I can’t use screws on Lego axles.

I attached a piece of double-sided tape on both sides of the propeller. That should give enough friction.

Then I pushed round Lego plates with axle holes (4032) to both sides.

Lastly I added Lego half-bushes (4265a) to the axle to keep the ends secure.

In hindsight, it worked perfectly. The connection never got loose in 10+ hours of diving under water.

The length of the propeller gave problems sometimes, as the blades would hit pool walls or rocks. I had to add a ramp up to the motor PWM control, to give less power for the first second you press buttons. That helped maneuvering in tight places.

Turn propeller

For the turn propeller, I selected Lego Propeller 3 Blade 3 Diameter with Axle Hole, part id 6041. You can get those for 0.1 euros from BrickLink: https://www.bricklink.com/v2/catalog/catalogitem.page?P=6041

The main criteria was small size. The blades cannot be long as they needs to rotate sideways near the back plate. Little thrust should be enough for turning the submarine.

As this is a Lego part, it was easy to connect it to the Lego axle. Bushes to both sides to secure it.

In hindsight, it was too weak. Or alternatively, the gear ratio I used was too small. The sub would turn too slowly. With the powerful main propeller, the controls felt unbalanced. You were always pressing left or right and sparely clicking forward or backward buttons.

Magnetic couplings

How to transfer rotation through the wall? With the first submarine I used a Lego axle and o-rings. It worked ok but leaked a little. In the next two submarines I used magnetic couplings with better success. So naturally I wanted to use the successful formula again.

The main benefit of a magnetic coupling is that it is 100% leak free. You don’t need to drill holes to the hull walls.

But it is more complex and requires more space from both sides of the wall. Also, the wall needs to be flat and thin.

Magnets

I’ll use 24 pieces of K&J Magnetics D38-N52 neodymium magnets. They cost 0.86 USD apiece, link: https://www.kjmagnetics.com/proddetail.asp?prod=D38-N52

The biggest selection criteria was diameter. These magnets are 3/16 inch (4.76 mm) thick, so they will fit almost perfectly in 4.8 mm Lego holes. I put a little piece of packaging tape around to keep them tightly in place.

D38-N52 neodymium magnet. On right side is one with packaging tape wrapped around.

The length is 1/2 inch (12.7 mm), so they will protrude nicely from a Lego gear (8 mm thick).

To hold the magnets, I’ll use a 40-tooth Lego gear for the main propeller and a 24-tooth Lego gear for the turn propeller. 8 magnets for the big gear and 4 for the small one. I set the polarity to change after each magnet. I have also tested “+ + – -” combination for the 40-toother, but it transfers less torque.

Note that the pull force between two magnets drops very rapidly with distance. The distance in my case is about 2.5 mm (2 mm plastic sheet + two tapes). Two D38-N52 magnets at 2.5 mm distance have 249 grams of pull force, but only 95 grams when you double the distance to 5 mm. That is why I used a thin plastic sheet for the lid. Magnet calculator: https://www.kjmagnetics.com/calculator.asp

Friction reduction

To reduce friction, I’ll use TapeCase 423-5 UHMW Tape. A roll 2 inches wide, 5 yards long, cost 17 USD, link: https://www.amazon.com/TapeCase-423-5-UHMW-Tape-Roll/dp/B00823JDXM

The friction coefficient should be 0.1-0.2 based on this source. I have also tried PTFE (Teflon) tape, which should have 0.1 friction coefficient, but it didn’t feel durable enough.

The tape is attached to both sides of the wall. The magnets will slide against the tape.

Tests

I did proper tests before going forward because I’ve had bad experiences. In Submarine 3.0 the magnets stopped rotating, as the tape would lose some of the properties in use. The problem was fixed when I removed two magnets to reduce the pull force.

It is a sensitive balancing act. With more pull force the friction becomes too high and the coupling stops rotating. With less pull force the torque transfer is too low, and you’ll see the secondary coupling stay in place while the primary coupling keeps rotating, which will cause bouncing and rattle as the magnets attract and repel.

So, I tested the setup properly in water and measured RPM as well. At first the main propeller rotated at a low 200 RPM rate, the motor being almost stalled. The turn propeller rotated at 500 RPM which was ok.

Then I added silicone spray on the magnets. The one I used is this: https://www.motonet.fi/fi/tuote/6003442/AT-HD-Silikonispray-400-ml

That increased the main propeller RPM from 200 to 300. I was quite happy with the result. But does it degrade under water? I tested it multiple times and let the surfaces dry and tried again the next day. No degradation. Why on earth didn’t I try lubricants before with my earlier submarines? And why is the low-friction tape performance so bad when not lubricated?

Lastly I also replaced a liftarm with a Lego Technic brick to hold axles and gears. That increased the rpm up to 375. I guess those tiny uneven surfaces on the liftarm introduce extra friction.

Just for comparison, the propeller rotates 450 RPM without magnetic coupling, and 375 RPM with magnetic coupling. So not many RPM’s are lost to friction. Good efficiency.

If I stop the propellers with my hand, the motor will keep rotating while the magnets bounce. This happens both with the main and turn propeller. So the couplings are too weak for the motor max torque. Not the way I’d have wanted it, but I didn’t have any more magnets slots available. In the end, is worked well in actual usage.

Build

After gaining confidence from the tests, I continued building the lid. I first attached a low-friction tape on both sides of the lid.

Then added silicone lubricant on the magnets. I used a Q-tip to avoid messy stains.

Added magnetic couplings.

Inserted a Lego Technic brick. Added a 8-tooth gear (10928) and an axle for the turn propeller. Then a connector piece (48496) to make an L-turn with two bevel gears (6589).

After inserting the bevel gears, I got a 3:1 gear ratio for the turn propeller. Added a 8-tooth gear, a long axle and the prop, and got a 5:1 gear ratio for the main propeller.

I used toothed half-bushes (4265a), that have in my experience the highest friction with Lego axles. Also, I hand-picked axles that seemed to stick tightly with the bushes. This was because I’ve had problems with axles falling off in Submarine 3.0.

Nothing holds the construct in place other than magnets. In my previous subs I had additional supports, but this time I went with this simpler construct. In the 10+ hours of testing it under water, it worked well. One time the big magnetic coupling bounced into a misaligned position, and I had to lift the submarine out of water to fix it. But even in that case, the construct didn’t fall off, which had been my biggest fear when diving close to branches and rocks.

One little problem I found in swimming pool testing, was that the submarine veered left. After investigation I found that the main propeller axle tilts when torque is applied.

I added two Lego liftarms on the shaft to support it better. Thereafter the submarine moved straighter.

RC Submarine 4.0 – hull (3/10)

2 Replies

In previous submarines I used IKEA glass food containers and a plastic lemonade pitcher as hulls. Both worked ok, especially the IKEA snap-on lids were easy to use. But this time I wanted to try something different.

Design

The hull has to be rigid. It cannot bend or compress under pressure, otherwise I’ll lose buoyancy, like with the Submarine 2.0 soft lids.

It needs to be transparent. I couldn’t even imagine building the thing, solving problems and checking everything is ok before dive if I couldn’t see inside.

It is good to be streamlined. As narrow as possible to increase movement speed. But also, I want an on-board camera to the front, so the front has to be flat. The back also needs to be flat for the large magnetic couplings I’m going to use. Not optimal for speed, but fast enough in hindsight.

So, I decided to make a cylinder with flat ends. Plain and simple.

What is the minimum diameter? Magnetic coupling for the main propeller will take 4 cm, as I intend to base it on a 40-tooth Lego gear, like in previous subs. For left/right turn I first envisioned using a linear magnetic coupling, similar to Submarine 3.0, but in the end I changed it to a 24-tooth magnetic coupling. That will take 2.5 cm. Some clearance is needed between the magnets. For the lid sealing I will use a piece of a smaller cylinder that is attached to the flat lid. The lid sealing takes 2 mm and the hull thickness is 3 mm. With this information I draw a sketch with 10 cm outer diameter. In this drawing everything would fit but with little room for error. Therefore I increased the diameter to 11 cm, just to be on the safe side.

What is the minimum length? The syringe takes 23 cm when fully extended. The hose connection to the syringe will take some space also, so a total of 25 cm, a good round number. I was able to find a 25 cm length cylinder. In the end, the hull was a tad too short for the syringe. It extended only 45 ml out of 60 ml, as it would hit the lid supports. Well, good enough.

Acrylic plastic cylinder

I bought two Perspex acrylic plastic cylinders: 11 cm and 10 cm in outer diameter, length 25 cm for both, thickness 3 mm for both. They cost 36 euros + postage. I bought them from an eBay seller simplyplastics, United Kingdom, links:
https://www.ebay.co.uk/itm/254457326025, https://www.ebay.co.uk/itm/254457319827

Left cylinder is the submarine hull. Right cylinder is for the lids. The right one is shorter as I’ve already sawed two pieces from it.

I used a hand saw to cut 2 cm pieces from the smaller cylinder. These were to be used with the lid.

Acrylic plastic is sensitive material. I quickly got some scratches on it and started using soft fabrics underneath the table/stool when working with it.

It also stains easily and is difficult to clean. I first used gloves that left smudge on the cylinders. You couldn’t remove the stains with soap and water. I first though about using a nail polish remover (acetone), but after I used it on the first lid, the plastic lost transparency. So not that. I read some debates online whether you can use alcohol to clean acrylic or not. I decided to try car windshield cleaner (ethanol alcohol, probably 50% concentration), first on scrub pieces. I didn’t see any cracks or clouding, so I used it in a few small places to clean the cylinder. I really dodged a bullet there, because I later found a chart that says more than 30% concentration of ethanol is not recommended for ANY use with acrylic.

SAN plastic sheet

I bought a plastic sheet made of styrene-acrylonitrile, also known as SAN. Thickness 2 mm thick and size 30×30 cm. It cost 5 euros in a finnish hardware store Biltema, link:
https://www.biltema.fi/rakentaminen/muovilevyt/muovilasit/muovilasi-2000030030

The plastic sheet placed at the front of the cylinder.

I wanted the lid thickness to be as small as possible for magnetic couplings. Less gap means more torque. But of course it shouldn’t bend or break. The 2 mm sheet felt rigid enough in my hands when I tried it before buying.

How to cut round pieces from a plastic sheet? I first cut a smaller square piece from the sheet. Then I attached a Lego pin (2460) to the center using double-sided tape.

The pin was then connected to a bunch of Lego bricks. The bricks were clamped tightly on a stool. I clamped a Dremel Lite to the stool leg.

I started cutting with a 561 MultiPurpose Cutting Bit. I could just rotate the sheet and let the machine do the work. It had to be done slowly because otherwise the plastic would melt and widen the cutting gap. It was noisy and tedious work.

I didn’t need to sand the edge, as the result was fairly smooth.

When I took off the double-sided tape, some glue was left on the center. Also there were smudges from the gloves I had used. I tried to remove them with acetone, but that clouded the sheet. With direct light you can really see the imperfections.

Ok, let’s make another one. This time I’ll use different gloves. Instead of acetone I use ethanol-based windshield cleaner. As I said before, you really shouldn’t use ethanol for acrylic, probably not good for SAN plastic either, but it worked for me.

The second one was a lot better. Here are the round sheet and the cylinder piece on top of each other.

Gluing plastics

I’ll need to glue SAN plastic to acrylic plastic. How do you do that? The bond needs to be watertight and unbreakable.

I decided to perform a little test with four glues:

hot glue
Biltema silicone (a cheap brand)
Bison plastic adhesive
Gorilla super glue

I cut small pieces from the left-over lid sheet. What I am testing here is SAN plastic against SAN plastic.

I let it dry 24 hours in my balcony at 10-20 degrees Celsius.

The results:

hot glue was ok, I needed pliers to remove it
silicone was a joke. It came off clean using my fingers.
plastic adhesive was tough. With pliers and some force I could separate the pieces.
super glue was the best. The glue bond never broke, instead the top piece snapped in half when I twisted it harder with my pliers.

So, super glue it will be. It should work very well with acrylic plastic also, so I’ve read. The ingredient of super glue is called cyanoacrylate.

I poured super glue onto the cylinder edge and pressed the lid on top of it.

This is how it looked 24 hours later.

What is that? It took some investigation online to know it is called frost, fog or clouding, and it is caused by super glue vapor. You need more ventilation.

For the second lid I blew air to it with a big fan. I did this for 24 hours after I glued it. The result was virtually fog free.

I didn’t want to threw away the first lid that had fog on it. The fog is just on the surface, so maybe polishing will remove it? I bought some Dometic Acrylic Glass Polish.

The result was surprisingly good. It required just a few minutes of scrubbing with a cloth and virtually all the fog came off. Although very good, it wasn’t 100% as transparent as the original, therefore I decided to use this lid for the back side.

I tested both the lids for waterproofing. One little spot on the glue seam leaked just a hair. I put more super glue to the spot and there was no leaks thereafter.

Sealing

The next thing was to seal it watertight. Here is an image of the starting point. Between the lid cylinder and the hull cylinder there is a 2 mm gap.

I first tried window seals. I tested EPDM E-profile, EPDM P-profile and silicone P-profile. I did tests where I filled the 25 cm cylinder full of water and let it stay one hour to see if any water leaked through the seals. Sometimes they didn’t leak at all, but sometimes a few drops went through, especially where the sealing was disconnected. Also, the adhesive would come off after opening and closing the lid multiple times. Not reliable.

Testing an EPDM P-profile window seal between the 2 mm gap.

I experimented with all strange combinations, adding pipe tape, Theraband rubber sheet or Glidex grease on the window seal. Nothing worked well.

Then I bought o-ring cord. Two meters of 2.5 mm diameter, NBR nitrile rubber, 70 shore hardness. It cost 18 euros in a finnish hardware store, link: https://www.ikh.fi/fi/o-rengasnauha-nbr70sh-2-50-orn25

I would have wanted something softer, but couldn’t find any. 70 shore is quite hard and will make installing it more difficult.

I measured the o-ring length. I read it should be 0-5% shorter than needed, to give it a small stretch. But I had trouble installing that, so I cut it 15% shorter. 2.5 mm is a bit too thick for 2 mm gap, so stretching will make it thinner and easier to install.

I made the cut using a thin knife, good for making it straight.

Glued it with super glue. That should be the best glue for bonding nitrile rubber.

I used an angle iron to keep the ends aligned when gluing them together.

Making the alignment correctly is important. At first I used just my hands when gluing the ends together. It resulted in the following image where a water bubble has formed at the slightly misaligned connection point. The alignment error is so small you can’t see it without looking very closely.

The o-ring is installed just by pulling it over the lid cylinder. There is no groove. Nothing keeps it in place, other than friction from the stretch. I though about adding some supports, but couldn’t think of any simple way.

Installing the lid was difficult. You would have to push the o-ring to the gap with your fingertips, otherwise it would slide back. Eventually I became quite good at it. The key was to enter the bottom side of the lid first, and then push the whole lid downwards hard, squeezing the o-ring, while turning the top side in.

To help sliding the o-ring in, I smoothed the cylinder edges with a Dremel 414 Felt Polishing Wheel.

I had some reservations about high pressure. Could the o-ring slide out of the gap when submarine dives deep? Or will it start to leak in high pressure? My previous subs had a better design, as the lid would press the seal tighter when outside pressure increases. Not this time. If there are leaks, they will increase when the sub goes deeper. In the end I tested the submarine only in 1.5 meter depth. In those low pressures it never leaked.

I tested the submarine 10+ hours and the o-ring worked surprisingly well. The interior was always dry after dive. After having so much initial trouble with the window seals, I really thought sealing would be the Achilles’ heel in this submarine. I’m glad I was wrong.

Holes

I need a hole in the walls to move water in and out of the syringe. Also, I need access to the outside water for measuring depth. First I thought using the same hole for both purposes, but when I tested the idea, I noticed 30 cm error in depth readings when the syringe was sucking water in. Better make two holes.

I used a 3.5 mm metal drill bit for drilling two holes to the SAN plastic lid. It drilled easily, no cracks formed.

I pushed two silicone tubes through the holes. I used 4mm diameter Lego pneumatic hose, part id 21825. Pushing them through the narrow hole was difficult. I had to carve the end of the hose sharp to help with the task.

Pressure equalization

Later I became aware of a problem with the internal hull pressure. When I installed the lids, the air inside the hull would push back and prevent me from pushing the lids fully to the end.

There is a 3 mm gap between the lid plate and the rim of the hull cylinder. This happens when internal pressure is not equalized.

I could extend the syringe fully before installing the lids. Then when I retracted the syringe, under pressure would suck the lids inside. But there was still a little gap left.

The gaps were problematic, as they would increase hull compression and therefore the submarine would lose buoyancy. Not a good situation when you try to build an accurate depth control.

Eventually I found a solution. An ugly trick but it worked. I would use a thin screwdriver to squeeze the silicone hose. That would form a temporary hole to let air out and equalize the pressure.

Equalizing internal pressure with a screwdriver.

It worked, but I wish there was a better solution. Either there should be a separate hole for equalization that you can conveniently open and close, or the caps should have a locking mechanism that doesn’t care about the internal pressure.

Frame

A particular puzzle task is to fit all parts inside the small hull cylinder.

The propeller motors have to be located at the back side to connect with the magnetic couplings. Their location is pretty much set in stone.

The syringe needs space to extend fully, so it has an option to go under or above the motors. Well, the bottom will be filled with extra weight, so let’s put the syringe on top.

The battery is very big. The most available space is near the front lid, so let’s put it there. You can conveniently switch it on/off with you finger when you open the front lid.

Some kind of frame is needed to hold it all together. I first build a long bar placed at the bottom, and everything else was connected to that.

However, when I got to add extra weights, the frame was in the way. In the second version, I raised the bar higher, and put little legs to both ends. This way there would be room at the bottom for the extra weights.

Tungsten pellets

Okay, how about extra weight. In submarine 1.0 I used steel plates, which need to be correctly sized to fit inside the hull. Not the best option when you are developing and changing things all the time. In submarines 2.0 and 3.0 I used lead pellets. Those are much better, as you can easily change the shape of the bag they are in.

But could I improve on the size? There is never too much free space inside the hull.

Tungsten is much heavier than lead. The tungsten alloy I’m using weighs 18 g/cm³, whereas lead weighs 11.3 g/cm³. So I’ll save 40% in volume.

I bought 2 kg of tungsten pellets, 2.5 mm diameter. It cost 236 euros in a Finnish gun store, link: https://www.aawee.fi/ammunta-ja-aseet/tungsten-18-0-5kg-2-50mm-hauli-tss-1-lk/p/TSS250/

That was very expensive. Tungsten pellets cost 10 times more than lead pellets. Not a good decision in hindsight. I could have used lead pellets without big problems.

Weighing

Later in the process, when all other parts had been installed, it was time to weigh and add the extra weight.

I calculated the hull volume. The hull cylinder diameter is 11 cm and length is 25 cm. Add 4 mm from the lids and some extra from the propellers to get a total length of 26 cm.

  V = π * h * r²
    = 3.14 * 26 * 5.5²
    = 2470 cm³

Displacement is the volume of water displaced by the submarine.

  m = V * ρ
    = 2470 cm³ * 1 g/cm³
    = 2470 grams

I weighed the submarine using a kitchen scale.

The base weight is 826 grams. Subtract it from the 2470 grams of displacement, and you get 1644 grams for the extra weight.

  extra weight = displacement - base weight
               = 2470 - 826
               = 1644 grams

But that would lead to neutral buoyancy. We want the submarine to be slightly positively buoyant, so that it floats when put into water, and the syringe ballast makes it dive.

The syringe ballast range is 42 ml. I think a good neutral buoyancy position is 30 ml, so that it has more range for surfacing, just to be safe. So take out 30 grams. The final number for extra weight is 1614 grams.

Put tungsten pellets in a plastic bag while measuring the weight.

Place the bag on the bottom of the hull.

The internal frame slides on top of the bag.

Now the weight is in the right ballpark. When you do the first dive in water, you need to tune it. I had to take out some 30 grams. Therefore the final extra weight was 1580 grams.

Bottom pads

I installed furniture pads underneath the hull to protect it from rocks. The lower front is probably going to have the most damage, so I put two pads there.

Safety line

In earlier submarines I attached a fishing line to the sub when doing dangerous dives. The fear is, the submarine could run out of battery, you could lose radio connection, the sub could tangle up with vegetation at the bottom, and surely a dozen other possibilities. You need a way to lift it from the bottom.

The problem with a permanent safety line is, it will get tangled in the propellers. You would be limited to just dive and surface.

So I invented a more convenient way. I’ll tape a magnet inside the top of the hull and another magnet to the end of the safety line. This way, I can use the submarine without the line attached. If the submarine is stuck at the bottom, I just lower the line, the magnet will snap onto the hull, and then I’ll pull it up. Of course, with this rescue plan, you need to always see the submarine position.

The magnets I used are neodymium ring magnets, K&J Magnetics R844-N52, link: https://www.kjmagnetics.com/proddetail.asp?prod=R844-N52

The line is Westline Dyneema 50, rated for 65 kg. It is the thickest fishing line I could find in a local hardware store.

I measured the pull force to be about 200 grams when there is hull wall and some felt pads in between. That wouldn’t be enough to lift the submarine if it is filled with water. So for the most dangerous dives I would still need a permanent safety line.

Captain

Oh, I almost forgot the most important part. The hero, the maniac, the brave soul to test this engineering deathtrap. Lego part id col154.

RC Submarine 4.0 – syringe ballast (2/10)

26 Replies

The first thing to build is a piston ballast. I’ll use the same syringe I did in Submarine 1.0, since it worked very well. It didn’t even leak 5 meters under water, so it should handle at least 0.5 bar of pressure.

The syringe

The syringe is a food syringe or marinade injector created by Eotia. It cost 4 euros in a Finnish hardware store, link: https://www.puuilo.fi/marinadiruisku-60-ml

The capacity is 60 ml. That should be enough based on my experiences with the first submarine. The hull volume will be roughly 2000 ml, so the syringe will have about 3% control range. Of course you would like to have more capacity to provide safety in case the hull leaks or compresses. Also, fine-tuning the extra weights to neutral buoyancy would be easier. But larger syringe also takes more space inside the hull, so it is a compromise.

Actually, Submarine 1.0 used only 25 ml of the available 60 ml capacity. That was because it was driven by a Lego linear actuator, part id 61927c01, which has a short range. The linear actuator is also weak, as it couldn’t empty the syringe in 5 meters under water. Max pressure for control was about 0.3 bar.

This time I want to try a different build, to get more range.

Build

I put gear racks, Lego part id 3743, alongside the syringe rod. A little grey part to the end to help hold the rack lines in place.

The biggest problem was to push gears hard enough against the racks so they won’t skip. I put two 8-tooth gears (10928) to push the rod from both sides, therefore supporting each other. They would also help to hold the gear racks in place. Luckily all dimensions were good so all parts fit very tightly.

Then I used one of the syringe handles to hold the gears in place lengthwise, to the direction of the rod. This required a lot of trial-and-error until I found a Lego part combination that wouldn’t break.

Lastly, I needed to gear down to provide enough torque. Lego gearbox, part id 6588, has a 24:1 gear reduction. It needs a worm gear (4716) and a 24-tooth gear (3648) to work. Besides increasing torque 24-fold, minus friction, it is very compact, perfect for a submarine.

The Lego motor I selected was EV3 Medium motor, because it has a tachometer inside that I can use to keep track of the syringe position.

I tested the construct for max pressure using a Lego manometer (64065). At first Lego parts slipped out of the handle when pressure increased to 0.3 bar. I put a Deltaco Velcro Strap around to pull the syringe and Lego parts together. That increased the max pressure to 0.5 bar. Beyond that I didn’t try, as parts started to bend dangerously, as you see from the image below. I did multiple tests to verify that it should work under 0.5 bar, which equals 5 meters of depth. But to be safe, I decided to limit the submarine depth to 4 meters.

The syringe speed is 2.7 ml/sec. Not very fast. Moving from min to max position (3 to 45 ml) takes 16 seconds. This will make the submarine react slowly to depth control changes. But as it turns out, it is enough for PID control.

So, the construct provides enough force. Actually too much. I had to be careful not to let the syringe run to the end, because it would probably break teeth from the gears. That was a problem. I briefly tried to replace 24-tooth gears with Lego clutches (60c01) that are designed to limit torque, but then the max pressure dropped to 0.17 bar. No good.

How to save the gears from destroying themselves? I resorted to tachometer data. I’ll read tachometer pulses and put limits to code on how the syringe is controlled. That was a source of annoyance further down the road, as Raspberry would not always catch tachometer pulses. Therefore I had to routinely check the syringe position data and tune it.

Wirings

Lego EV3 Medium motor is connected to Raspberry using a Lego EV3 cable. I cut one EV3 cable and crimped female DuPond connectors to the end.

Lego EV3 cable with DuPond connectors attached to the end.

The cable has 6 wires. M1 and M2 are for driving the motor. TACHO A and TACHO B for reading the tachometer pulses. GND and VCC are used to power the tachometers.

You should supply VCC with 5V according to the specs, but that is not an option, as the tacho pulses would have too high voltage for Raspberry to read them directly. Therefore I’ll supply VCC with 3.3V. I’ve used that setup previously with my Reaction Wheel Inverted Pendulum, and it works. I also have checked that the current draw from VCC is 6.5 mA, which is small enough for Raspberry.

Code

Then I wrote a Python code to read the tachometers. There are two tachometer signals and you know the direction of rotation based on which signal goes to 1 first. The code looks more complex than it should be, but that is because it provides the most reliability. E.g. you can’t read tachoA.value in the signal handler, because by the time that code is executed, the value has already changed. I tested multiple implementations with my Inverted Pendulum. As a side note, you could improve reliability by doing sleeps in the main control loop in small increments. I guess the rising/falling edge detection needs the CPU to be idle or something.

tachoPower = DigitalOutputDevice(20)
tachoA = DigitalInputDevice(16)
tachoB = DigitalInputDevice(19)

tachoAValue = tachoA.value
tachoBValue = tachoB.value
def tachoA_rising():
    global tachoCount
    global tachoAValue
    global tachoBValue
    tachoAValue = 1
    if tachoBValue == 0:
        #A in rising edge and B in low value
        #  => direction is clockwise (shaft end perspective)
        tachoCount += 1
    else:
        tachoCount -= 1
def tachoA_falling():
    global tachoAValue
    tachoAValue = 0
def tachoB_rising():
    global tachoCount
    global tachoAValue
    global tachoBValue
    tachoBValue = 1
    if tachoAValue == 0:
        tachoCount -= 1
    else:
        tachoCount += 1
def tachoB_falling():
    global tachoBValue
    tachoBValue = 0

tachoA.when_activated   = tachoA_rising
tachoA.when_deactivated = tachoA_falling
tachoB.when_activated   = tachoB_rising
tachoB.when_deactivated = tachoB_falling

tachoPower.value = 1

Syringe position is calculated from the tacho count. I included hysteresis calculation into the code to compensate for gear backlash and such things.

Syringe range was set to be between 3 ml and 45 ml. Minimum was 3 to provide some safety from hitting the end, and max was 45 because the hull doesn’t have more room.

SYRINGE_POS_MIN     = 3     #syringe ballast min pos [ml]
SYRINGE_POS_MAX     = 45    #syringe ballast max pos [ml]
SYRINGE_TACHO_COUNT = 19000 #tacho count from syringe min to max pos
SYRINGE_HYSTERESIS  = 360   #motor+gearbox+syringe backlash/hysteresis [tacho counts]

#calculate syringe position
if tachoCount > trueTachoCount:
    trueTachoCount = tachoCount
elif tachoCount < trueTachoCount - SYRINGE_HYSTERESIS:
    trueTachoCount = tachoCount + SYRINGE_HYSTERESIS
syringePos = SYRINGE_POS_MIN + SYRINGE_POS_MAX * \
             trueTachoCount / SYRINGE_TACHO_COUNT  #[ml]

I need to store the syringe position in flash, so that it is not lost during shutdown.

import configparser

#read config file
configFile = open("submarine_4.ini", "r+")
config = configparser.ConfigParser()
config.read_file(configFile)
tachoCount = int(config['default']['tachoCount'])

storedTachoCount = tachoCount
def writeConfigFile():
    global configFile
    global config
    global tachoCount
    global storedTachoCount
    if tachoCount != storedTachoCount:
        storedTachoCount = tachoCount
        config['default'] = {}
        config['default']['tachoCount'] = str(tachoCount)
        configFile.seek(0)
        config.write(configFile)
        configFile.truncate()

Here are the syringe motor output pins.

#prepare motor drivers
motorDive = DigitalOutputDevice(13)
motorSurface = DigitalOutputDevice(12)

Here is code for driving the motor. syringeMotorCtrl is the PID output. If syringe position is out of limits, the motor is not driven.

#syringe range limitations
if syringePos <= SYRINGE_POS_MIN and syringeMotorCtrl < 0:
    syringeMotorCtrl = 0
if syringePos >= SYRINGE_POS_MAX and syringeMotorCtrl > 0:
    syringeMotorCtrl = 0

#drive syringe motor
motorDive.value = (syringeMotorCtrl > 0)
motorSurface.value = (syringeMotorCtrl < 0)

I also added a deadband limit to reduce motor wear, as the PID output is noisy. More of that when testing PID.

Tests

Here is the syringe position data from a typical swimming pool test run. The submarine dives to the bottom, drives around, and comes back up. PID control is in use.

As you see, the active range for operation is about 25 ml. That is about half of the syringe size. Therefore the syringe is large enough for control.

In the 10+ hours of testing the submarine, the syringe worked physically well. I never noticed any leaks. Then again, it was tested only at a max depth of 1.5 meter.

Two times the range limitation failed and the syringe ran against the end. The first time the tacho count was wrong after a long test run. The second time I had made a change to the code that crashed it, leaving the syringe motor on. No teeth was broken in either case, but the gears slipped out of the gear rack and I had to install them again. The fact that Raspberry failed to catch tacho pulses and the actual syringe position would be lost in time, is the biggest complaint I can see with the syringe. It was annoying to need to check the position data and tune it between test runs.

RC Submarine 4.0 – background (1/10)

10 Replies

This project began from a simple question: how to keep a submarine depth at a constant level? Unsteady depth had been a nuisance with the three submarines I made earlier, especially subs 1 and 3. Those submarines were always either at the bottom or at the surface. A good challenge to overcome.

I thought PID control might resolve the problem. I had some previous experience using PID with a Reaction Wheel Inverted Pendulum, so I knew the basics and had some parts already available, e.g. Raspberry Pi Zero 2. I could buy a pressure sensor and use that as an input for the controller. The whole idea that you could measure depth with a pressure sensor felt very intriguing.

There were other things I wanted to try. For the hull I would try a see-though acrylic plastic cylinder, which would be narrower than the previous hulls and therefore faster underwater. I would make the end caps myself, which would enable me to use very thin material, good for magnetic couplings to transfer a lot of torque. The previous subs had a separate Li-Po battery for radio control, which I wanted to get rid off and use only one battery onboard. Extra weight had been steel plates or lead pellets in previous subs, but now I wanted to try tungsten pellets to save space for other stuff. So, a lot of new things, that’s the way I like it.

How to maintain depth?

I struggled with this question with my first submarine. What physical principle makes the submarine go to depth X? Then I realized, it doesn’t go to depth X automatically. You need to drive it there, the same way a car needs to be driven to position X by pressing the gas pedal and the brake pedal. The forces in a submarine are buoyancy and gravity. The depth is the result of these forces over time, following Newton’s second law. In mathematics, this would be a double integral: force / acceleration -> velocity -> position.

Buoyancy is an upward force caused by the surrounding liquid. It equals the weight of the liquid being displaced. If the submarine volume is 2400 ml, then the displacement is also 2400 ml. 2400 ml of water weighs about 2400 grams. To keep the submarine in neutral buoyancy, not moving up or down, the gravity should be equal to the buoyant force. In this example, the mass of the submarine, including the hull, motors, propellers, tungsten pellets and everything else, should be 2400 grams.

Note that the buoyant force does not depend on depth. If your submarine is 5 grams negatively buoyant, meaning it has slightly more mass than is being displaced, it will sink at the same downward force from surface to bottom. That assumes the submarine stays unaffected during the dive. If the submarine leaks, it’s mass will increase, which will make it sink faster. Or if the hull compresses under water pressure, it’s volume of displacement will decrease, which will again make it sink faster.

Respect the force of pressure

When you as a human dive in a swimming pool, you don’t feel the pressure. That’s why it is surprising how large it is. I had first hand experience of it with my Submarine 2.0 that had a soft plastic lid. I calculated that in 1.5 meters of depth, a normal swimming pool depth, the water pressure will press the lid with 35 kg force. That is like a frigging small human standing on it. No wonder it bent heavily. It resulted in a loss of buoyancy, which I measured to be 66 grams. That was enough to prevent the submarine from rising from the bottom, as the propellers didn’t have enough thrust force. The problem was finally fixed when I added red Lego beams under the lid to support it from bending.

Red Lego bars under the lid help it withstand water pressure.

How to control depth

I’ve tried three methods for controlling the depth of a submarine. Submarine 2.0 had propellers pushing water up or down. In this method the submarine is weighed to be neutrally buoyant. Gravity and buoyancy stay always the same while the propellers exert force. In my experience this is the easiest to control. You get fast response to radio control buttons. You see the submarine immediately start moving up or down, and when you stop pressing the buttons, it will quickly slow to to a halt due to water resistance. Maybe the only problem I see is that the weighing has to be very accurate, otherwise the sub is always sliding up or down.

The two red Lego constructs in front are the propellers for controlling depth. They rotate the opposite direction from each other to prevent yaw.

Submarine 3.0 was equipped with an air compressor and a balloon. When the balloon is filled with compressed air, it will expand and displace more water, and thus increase buoyancy force. In this method, gravity stays always the same. In my experience this is the most difficult to control. It takes time to inflate the balloon, at least when using inefficient Lego compressors, and therefore the response time from radio control buttons to actual movement is long. Moreover, you’re always at a loss where the neutral buoyancy point is. You need to guess from the sub movement whether to inflate or deflate the balloon. Even worse, as the balloon air compresses under pressure, you’ll lose buoyancy as it goes deeper. 60 ml balloon will shrink to 52 ml at 1.5 m depth, so you’ve gone from neutrally buoyant to 8 grams negatively buoyant while diving to the swimming pool floor. This will make the depth position inherently unstable.

The yellow balloon is inflated through the blue hose that connects to a compressor inside the hull.

Submarine 1.0 had a piston ballast to suck water in. Here a motor moves a syringe, which has a hose connected to outside the hull, through which water will move into the syringe chamber. In this method the sucked-in water acts as an extra weight that will increase gravity. Buoyancy stays always the same. This has a lot of the same problems as the balloon method, having a poor response time to buttons and difficulty to know neutral buoyancy position for the syringe. In one respect this is better, as the syringe will not compress under pressure like the air balloon does, leading to better stability.

Submarine 1.0 has a syringe to suck water in.

I’ll try to use syringe ballast for Submarine 4.0.

Why? For one reason, you can measure the piston position with an Lego EV3 motor that contains a tachometer. That will help the control loop. With an air balloon you would have to measure the deflation time, which would be less accurate. The best method out of the three would probably be propellers as in Submarine 2.0, but the hull shape would need to be less streamlined.

Ok, let’s start building.