A few months back I created a basic mobile platform using parts from an old 3D printer. It was fun but not very practical.
I designed Roverling Mark II so I could experiment with a practical, configurable, and reliable mobile platform. Hopefully, one day we get it to do some useful stuff around our bush block, like:
- Searching paddocks for weeds and recording locations.
- Navigating down a 200m drive to check if the gate is closed.
- Navigating up the drive 50m to check that the machine shed roller doors are secure.
- Recording animal sightings on the block. We’d see up to 50 'Roos' per day on our block.
- It would be good to identify and scare deer away from our olive grove.
- And a friend already wants to mount a metal detector and do automatic scanning of a large area.
To that end, here is the complete design and everything you need to know to make your own or any variation. As much as possible I’ve kept the cost down to a minimum, reused some older parts, and printed as much as possible.
This is a current work in progress. At some stage, I needed to draw the line and write something up before it got too big. So these are the current working and operational specifications:
- 440 mm long x 350 mm wide, 250 mm max height, 200 mm nominal platform height
- 3.5 kg without battery, uses an 18V power tool battery
- Up to 22 satellite GNSS receivers, augmentation using SBAS, 1Hz updates
- IMU: 3x accel, 3x gyro, magnetometer, pressure
- 2.4Ghz 6 channel RC receiver and decoder, with diverse receiver
- 2 channel motor quadrature decoders
- 2 channel motor current sensors (effectively torque sensors)
- Sonar, ranger
- 915 Mhz LoRa module. Comms at 1800 bps, 64 byte telemetry packet, 16 byte command packet, encrypted, super reliable
- LoRa base station, which is effectively my MQTT gateway for telemetry and commands back
- Mapping module runs on desktop from the MQTT feed to produce real-time tracking info overlaid on an image (for me an old NearMap screenshot).
- On board, RPi 4B w/ 4G running the latest OS with a camera 3 module. Work has just begun here, but object detection is working a treat, as does image capture and streaming.
All the 3D design is done in FreeCAD and all the slicing and printing on a Prusa platform. As well as the actual design files, I’ve also made available stl model files, g-code files, and Prusa project files for all parts.
The schematic is done with KiCAD. I used mixed prototyping methods and no PCB has been designed (yet) so you will need a fair understanding of how to layout and wire correctly.
All the code is in Python. I’ve pretty much written my own drivers for all of the low-level stuff except the sx1262 suite for LoRa. I’ve tried to document as much as possible.
All resources can be found here at my GitHub repo.
A cheap multipurpose mobile robotic platform for research, development, education, and fun.
Motors
Rather than steppers, I’ll be using brushed DC motors. Good ones, with planetary gears and optical encoders aren’t cheap, however, I have a few lying around from the 90s with suitable LM18200 H-bridges.
All of these were recovered from a SocBot platform, designed in 1999 by RMIT University to compete in the RoboCup games in Stockholm in 1999 and in Australia in July 2000.
This last SocBot has been lying around in storage since then and is starting to rust. It probably won’t make it into a museum, so parts it will become. The electronics are pretty old, a 68HC11 8-bit processor, running at 16MHz, programmed in assembly code, and a Xilinx 3000 series FPGA to achieve lower-level functions (kicker controls, quadrature decoder, PWM generation, etc). Now all of these are easily implemented on a $7.50 RP2040 MCU!
Back then these SocBots cost over $15k each to make.
The SocBot design was based on a 28.8V power supply (4x 7.2V NiCad RC racing packs), but there shouldn’t be an issue running at 18V, just less torque and speed. The SocBots were pretty heavy, at least 15kg, so if I can make this one lighter, the reduced voltage and subsequent torque shouldn’t be a problem for the acceleration I have in mind.
The motors I have recovered are Maxon 2332.909, 11W, 24V, diam 32mm, 17.83:1 planetary reduction with 100 CPT quadrature optical encoder. At 24V stall torque is 75 mNm * (18V/24V) * 17.83 ratio = 1003 or close enough to 1 Nm per motor at 18V - nice. The measured weight is 340g each.
- Motor Specs (from 2000)
- Gear Specs (can’t find actual specs from 1999, but this is very similar): part number 166159
Hubs (1x front left, 1x front right, 2x rear)
Rather than a custom design for the motor mount, I have decided to use a more universal approach to make it easier for others to follow with different motors, given that they are the most expensive components.
My particular gearhead matches one of the profiles on the ‘Motor Mounts For Andymark Neverest Motors - 1702 Series Quad Block Motor Mount ‘ (GB-1702-0032-0001) mounting bracket (shown in red below). So I designed my hub to suit, which should mean that the other motor mounts should also be compatible with this hub, as long as the motor diameter is less than 32mm.
In the next pictures, steering components are described later.
Rear Hub
A simple printed piece with two 6x15x5 bearings, and attached to the suspension/frame with two 8x150 SS rods.
Tyres & Wheels
I’m making these tyres with a greater section profile, less infill, and less perimeters compared to version 1. I also designed, I think, a better tread profile, which also requires less support material to print.
In the wheels, I have included a captured nut and set screw so that it is easier to secure to the motor’s D-shaft.
The front wheels have more space on the inner side to fit the motor, whereas the back wheels have an extended collar.
Right and left tyres are a mirror image of each other, achieved at the slicing stage, rather than with FreeCAD. Fit the tyres so the ‘arrows’ on the tread point to the main direction of travel.
Steering
I’m not sure if it’s a done thing, but I used the constraint-based sketcher in FreeCAD to dynamically simulate movement in the design. You basically set up all the constraints but don’t fully constrain them. I then drag the linkages around and see the effect. This allowed me to simulate many different design variations before committing to anything. It’s not a perfect tool, but for what it costs it can do amazing things.
I’ve left the simulation ‘part’ in the FreeCAD project, so take a look if you want.
Rather than trying to reinvent from scratch and fail, I’ve decided upon a proven Ackerman Steering Geometry with the following characteristics:
- Kingpin centres: 200 mm x 300 mm
- Steering rod to kingpin centre length: 35 mm
- Tie rod length 177.3 mm for about a 0.5-degree toe-in on each side
- Ackerman angle 18.44 degrees
- No camber or caster angle at this stage.
- Linkages aft of the wheel, initial designs had it forward.
I’ll be using a standard-size RC servo to control the steering, but rather than use this to linearly move the steering rod, I’ll mount it to one wheel hub so it effectively fixes the angle between the steering rod and tie rod. Between 60 - 180 degrees gives full steering in both directions. The servo I have at hand is rated at 0.3 Nm (3 kg.cm) which can easily be replaced with a 1 Nm unit if required.
The front left hub also serves as the mounting point for the servo, with the shaft output directly in line with the calculated Ackerman geometry.
Important note: I have used the normal left/right car-based nomenclature to identify the sides. This is NOT consistent with that used in FreeCAD, as it is based on looking from the outside of the object rather than from within.
Suspension
Independent. A very simple sliding pillar design. Each hub has one or two 8mm vertical SS rods securely fastened to the hubs. The front ones are effectively the kingpins.
Then starting from the bottom working up:
- Printed hub
- Steel shim 8x11x1
- 2x Stainless Springs 1x10x40
- Steel shim 8x11x1
- Linear Bearing 8x15x24 - embedded in chassis corner
- Steel shim 8x11x1
- Printed Flex Stop
- Clamping Collar 8x21x9
Pics show unloaded and with a 10kg payload.
Chassis
In my last design, I used 8mm steel rods that added significant weight. This one uses a plain old 10 x 1.5 mm aluminium rod.
The corners contain the linear bearings and affix to the axial and radial aluminium rods. The design allows me to extend the platform on the front for sensors whilst using the back to secure the battery pack to help distribute the weight.
There is a generic platform with holes on 20mm centres for mounting electronics and other accessories.
In the pictures, you will also see my battery adapter, which has been modified from the previous design to fit this chassis.
Model
Print Parts
I usually print with generic brand 3D filaments and until now I haven’t really had a problem with them. In this picture, the black parts are Bilby 3D brand PETG, the clear is eSun PETG and all the other colours are Bilby 3D PLA. The black stuff is very brittle, more so than any of the PLA’s.
Moving forward this entire project will be printed in eSun branded filament as I have had much success with this brand. I have no commercial association with eSun.
In the sliced project and G-code file names, Rn denotes the revision number (n) of the slicing and printing attempt, not the design revision. So R205-Hub-FL-R3.3mf means Roverling design version 205, of the Front Left Hub, and Revision 3 of the slicing and printing parameters.
4x Tyres R3
- Material: eSun eLASTIC Flexible TPE in black
- Generic setting: 0.3mm draft
- Perimeters: Top:6, bottom:5, perimeters:3, avoid perimeter crossing
- Infill: 3% octagram spiral
- Top/bottom fill pattern: Archimedean chords
- Support: make lighter and further separation. It is very easy to remove, just grab it from the centre and pull it off quickly - like a bandaid.
- And importantly, limit the maximum volumetric speed to 3.5mm^3/s On the i3 Prusa I used the provided SemiFlex profile, but increased the volumetric speed by around three-fold without printing issues. About the only change I make to the printer is to loosen the feed pressure screw substantially.
- There is also a ‘tyre label’, which I merge with the main tyre to provide the writing.
- Within the slicer, I have mirrored two of the prints for the opposite side. Mainly to ensure that the bottom/top of the print is the same on each side and the writing is on the same side. Not entirely necessary.
- Inside the tyre is nearly all air, only 3% infill to hold up the top
2x Front Wheels R2 & 2x Rear Wheels R1
- Material: eSun PETG in solid orange
- Generic setting: 0.3mm draft
- Perimeters: top: 4, bottom: 3, perimeters: 5
- Infill: 20% grid
- Avoid perimeter crossing
- Top/bottom fill pattern: concentric
- Generate support material everywhere
- Note that one of two runs might be left hanging to the top rim edge. Just chop them off as nobody will see anyway. It’s not worth creating support just for this.
- Don’t fit the tyres to the wheels yet
1x FL Hub R3 & 1x FR Hub R2 & 2x Rear Hub R1
- Material: eSun PETG natural
- Generic setting: 0.3mm draft
- Perimeters: top: 5, bottom: 5, perimeters: 6
- Infill: 10% rectilinear
- Top/bottom fill pattern: concentric
- Generate support material everywhere
- Don’t attach anything yet.
2 x Front Chassis Corners R5, 1 x Back Right & 1x Back Left (mirror) Chassis Corners R2
- Chassis Corners. These were very strong in the last design as the fill was 100%. This also made them heavy. The new design aims for about a 1.5 mm wall thickness with minimum infill.
- Material: eSun PETG natural
- Generic setting: 0.3mm draft
- Perimeters: Top: 6, bottom: 5, perimeters: 6
- Infill: 10% rectilinear
- Top/bottom fill pattern: rectilinear
- Then carefully clean up the supporting material. Don’t drill out 10mm holes, just use a 9.5mm drill, by hand, to remove support material.
- The rods need to fit very tightly - you will need a hammer to get them in, BUT NO MORE than 30mm. And then a pair of pliers to get them out - twist and turn as you go. Same with linear bearings. But don’t do this step now. Once in they should stay in.
1 x Platform R2
- Hard to stick down at the edges at bed 80 degrees, so up to 90+ degrees and slow the first layer down to 80%
- Material: eSun PETG natural
- Generic setting: 0.3mm draft
- Perimeters: Top: 4, bottom: 3, perimeters: 8
- Infill: 20% grid
- Top/bottom fill pattern: monotonic
- Generate support material
4x Flex Stop R1 (shock absorber)
- Material: eSun eLASTIC Flexible TPE in black
- Generic setting: 0.3mm draft, no special settings, no support
- Limit maximum volumetric speed to 3.5mm^3/s
1 x Servo Horn Adapter R2
- Material: eSun PETG natural
- Generic setting: 0.3mm draft
- Perimeters: top: 8, bottom: 8, perimeters: 6
- Infill: 100% rectilinear grid
- Top/bottom fill pattern: monotonic
- Generate support material
1 x Battery Carrier R1
- Material: eSun PETG in silver (look grey to me)
- Generic setting: 0.3mm draft
- Perimeters: top: 4, bottom: 4, perimeters: 4
- Infill: 20% grid
- Top/bottom fill pattern: monotonic
- Generate support material everywhere
Procure Parts:
2x Brushed DC Motor, around 10W, maximum diameter 32mm
1x Standard RC Servo (torque requirement TBD)
1x 10mm x 1.5mm x 1m Aluminium Rod, cut into 4 pieces (2x180, 2x280)
Nx M3 & M4 Fastening Hardware
8x Stainless Compression Spring (1x10x40) ideally with a spring rate of 1 N/mm
1x Stainless Steel Threaded Rod (M4 x 0.7mm, 200mm Length)
12x Stainless Steel Shim (8mm ID x 11mm OD, 1mm Thickness)
4x Stainless Steel Shim (6mm ID x 9mm OD, 1mm Thickness)
4x Stainless Steel Threaded Plate (M4 x 7 x 7)
4x Stainless Steel Round Shaft (8mm Diameter, 150mm Length)
2x Stainless Steel D-Shaft (6mm Diameter, 90mm Length)
2x Quad Block Motor Mount
4x Linear Ball Bearing (8mm ID x 15mm OD, 24mm Length)
4x Flanged Ball Bearing (6mm ID x 14mm OD, 5mm Thickness)
2x Aluminium Clamping Collar (6mm Bore, 8mm Length)
4x Aluminium Clamping Collar (8mm ID x 21mm OD, 9mm Length)
1x Steel Ball Linkage (Female M4 x 0.7mm, 24.1mm Length)
Assembly
No particular order has been formulated, as I’ve rebuilt and remade a few times. Below are some of my recommendations & techniques. Otherwise, I hope the pictures tell a thousand words.
Chassis
Chassis first. Linear bearings into corners, line up, and use a vice. It MUST be aligned otherwise you’ll break something.
Make sure the 10mm holes are very clear. The aluminium rods get hammered in - support well and ensure proper alignment. But don’t do them yet.
Platform first as it is very tight. Bang the side rails in from the end, protecting the metal, until they protrude evenly from both sides.
Then assemble to short edges, using the installed platform rods as a size guide. I used a hammer to press them in but a vice or clamp might be better.
If you are going to use my battery adapter, you need to install this first on the rear rod.
Then press the small assemblies onto the platform section and voilà. Centres need to be accurate (200 x 300 mm).
I drilled extra holes and used M3 x 30 CS bolts to prevent rods from turning and twisting the platform, just in case.
Wheels
Install M4 x 7 square nut after drilling out hole to 4.5mm
Method for front and back wheels:
Method for back wheels:
Put together the rear hub assemblies, referring to the pictures in the Hub section. Attach rear wheels to the hub through bearing and shim and lock in place with a collar.
For the front assemblies carefully press motor shafts into the front wheels. I used the hub for leverage.
Tighten all 4 shaft locking screws
Fit all 4 hubs to corner brackets, using shim, spring/s, and shim, then pass through the linear bearing, shim, flex stop, and finally locking collar.
Next tyres. Use a small screwdriver as a tyre lever, but don't press too hard on the thin wheel rim, instead leverage from the middle where damage won't be seen. The locking nut should almost be flush, so the flex will slip over it.
And finally the steering components. Again check previous pictures for the servo horn adapter, threaded rod and ball linkage arrangement. Don’t do tyres first as you may damage the steering.
That's the assembly complete, except for these last-minute fixes. A larger steering servo requires this section to be sliced off. Also need a redesigned servo horn.
Camera Bracket/Mount
This is probably the least documented section. I started with a list of actual IOs that I think I needed and then split these logically. I ended up with an RP2040 processor for motor control, motor sensing, steering control, and an RC interface. This can, at the lowest level, run independently, using the RC just as a remote control.
The second RP2040 handles the telemetry, comms, and all the other peripherals.
| Module | Pins Required | Protocol Requirements |
| RP2040-Zero | 21 / 24 | Processor M - Motion |
| Motor Drive LM18200 | 6 | PWM, Logic |
| CPT Encoder | 4 | PIO Decoder/Interrupts |
| Steering Servo | 1 | PWM |
| RC receiver | 6 | Pulse / Interrupts Both Edges |
| Motor Current Sense | 2 | Analog |
| Inter MPU Comms | 2 | UART 1 |
| Free | 3 | |
| RP2040-Zero | 18 / 20 | Processor T - Telemetry |
| LoRa | 7 | SPI 1 / Logic / Interrupts |
| GNSS | 2 | PIO UART / Interrupts |
| IMU | 2 | I2C 1 |
| PIR | 1 | Logic |
| Inter MPU Comms | 2 | UART 1 |
| RPi Comms | 2 | UART 0 |
| Battery Voltage Sense | 1 | Analog |
| XL-MaxSonar-EZ0 Sonar Range Finder MB1200 | 1 | Analog |
| Free | 2 | |
| Raspberry Pi 4 Model B 4GB | 5/28 | Processor R - Control |
| RPi Camera | 0 | RPi4 CSI Camera Port |
| Inter MPU Comms | 2 | UART |
| RP2040-Zero | 7/28 | Processor T - Telemetry |
| LoRa | 7 | SPI 1 / Logic / Interrupts |
This is probably the least documented section. I started with a list of actual IOs that I think I needed and then split these logically. I ended up with an RP2040 processor for motor control, motor sensing, steering control, and an RC interface. This can, at the lowest level, run independently, using the RC just as a remote control.
The second RP2040 handles the telemetry, comms, and all the other peripherals.
| Module | Pins Required | Protocol Requirements |
| RP2040-Zero | 21 / 24 | Processor M - Motion |
| Motor Drive LM18200 | 6 | PWM, Logic |
| CPT Encoder | 4 | PIO Decoder/Interrupts |
| Steering Servo | 1 | PWM |
| RC receiver | 6 | Pulse / Interrupts Both Edges |
| Motor Current Sense | 2 | Analog |
| Inter MPU Comms | 2 | UART 1 |
| Free | 3 | |
| RP2040-Zero | 18 / 20 | Processor T - Telemetry |
| LoRa | 7 | SPI 1 / Logic / Interrupts |
| GNSS | 2 | PIO UART / Interrupts |
| IMU | 2 | I2C 1 |
| PIR | 1 | Logic |
| Inter MPU Comms | 2 | UART 1 |
| RPi Comms | 2 | UART 0 |
| Battery Voltage Sense | 1 | Analog |
| XL-MaxSonar-EZ0 Sonar Range Finder MB1200 | 1 | Analog |
| Free | 2 | |
| Raspberry Pi 4 Model B 4GB | 5/28 | Processor R - Control |
| RPi Camera | 0 | RPi4 CSI Camera Port |
| Inter MPU Comms | 2 | UART |
| RP2040-Zero | 7/28 | Processor T - Telemetry |
| LoRa | 7 | SPI 1 / Logic / Interrupts |
Parts List
1x Adafruit Ultimate GPS Breakout with GLONASS & GPS
1x SMA to uFL/u.FL/IPX/IPEX RF Adapter Cable
1x Makerverse Project Proto - Protoboard with Power Breakouts
6x 2.54mm Pitch 4-Pin Screw Terminal Block
2x RP2040-Zero
1x Raspberry Pi Pico W (for LoRa / MQTT gateway)
1x GPS/GNSS Magnetic Mount Antenna SMA - 3m
1x AltIMU-10 v6 Gyro, Accelerometer, Compass, and Altimeter
2x SX1262 LoRa Node Module for Raspberry Pi Pico, LoRaWAN
1x 915MHz LoRa Antenna RP-SMA - 1/4 Wave 2dBi
1x PIR Infrared Motion Sensor (HC-SR501)
1x XL-MaxSonar-EZ0 Sonar Range Finder MB1200
1x DC-DC Adjustable Step-down Module 5A 75W
1x Raspberry Pi 4 Model B 4GB
1x Raspberry Pi Camera Module 3
2x LMD18200, 3A, 55V H-Bridge, or similar
Schematic
See https://github.com/MarkMakies/Roverling-Mk-II/tree/main/Electronics/R202 for the latest schematic
GNSS Antenna and Base
Made from an old film tin lid, 100 mm in diameter, a printed cover, and a base that also forms the battery adapter clip. Also serves as a good place to hide the extra cable length.
PLA is fine, nothing special is required, just the basic 0.3 mm draft template. You will need support for the base/battery clip combo to hide the cable.
The tin is easy to solder (ground plane connection) but you must protect the cable from the sharp edges. I use hot glue, lots.
RC Receiver
A plastic holder protects the wires from breaking and the receiver plugs into these female sockets.
RC Satellite Antenna
LoRa 915MHz Antenna
Breaking Out More RP2040-Zero Pins
Wiring
LoRa Base Station & MQTT Gateway
You can find a wiring diagram here. If you are using a Pico W, be warned that mounting it as designed (one on top of the other) brings the RC cans/components too close together. This will result in between a 10 to 20 dB drop in receiving SNR as you start to go below -70 dBi RSSI.
You’ve been told - it took many days to figure this out.
LoRa Base Station & MQTT Gateway
There are multiple Python modules that need to be loaded into the three RP2040s. Everything you need to know is on the GitHub.
Basic block diagram
Motion Processor
Works independently of other modules. Can use RC to control without any other modules. On RC toggle FLAPS to put it into RC mode. The onboard LED will change from steady green to flashing pink. Ensure throttle is at min first. Ailerons control steering, elevator controls camera tilt. GEAR puts it into reverse mode.
################################################################################
# Roverling Mk II - Motor Control Processor
print('Roverling Mk II - Motor & RC Control main.py on RP2040')
from machine import Pin, PWM, ADC, UART
from time import sleep_ms
from gc import collect
import neopixel
# my modules
from RCinterface import dataR
from QuadDec import dataV
from Comms import SendTelemetry, RecvCommand, dataCMD
Muart = UART(1, baudrate=1_000_000, tx=Pin(8), rx=Pin(9))
################################################################################
# LED
NeoPin = Pin(16,Pin.OUT)
Neo = neopixel.NeoPixel(NeoPin,1)
LastLEDstate = True
RGBon, RGBoff = (20,0,0), (0,0,0)
Neo[0]=(20, 0, 0)
neopixel.NeoPixel.write(Neo)
def LEDupdate(mode='none'):
global Neo, LastLEDstate, RGBon, RGBoff
if mode == 'GreenSolid':
RGBon, RGBoff = (20,0,0), (20,0,0)
elif mode == 'RedSolid':
RGBon, RGBoff = (0,20,0), (0,20,0)
elif mode == 'YellowFlash':
RGBon, RGBoff = (6,20,0), (0,0,0)
elif mode == 'PinkFlash':
RGBon, RGBoff = (5,50,10), (0,0,0)
if not LastLEDstate:
Neo[0] = RGBon
LastLEDstate = True
else:
Neo[0] = RGBoff
LastLEDstate = False
neopixel.NeoPixel.write(Neo)
LEDupdate('RedSolid')
################################################################################
# Motor Current
Lsens = ADC(Pin(28))
Rsens = ADC(Pin(29))
def GetMotorI():
global LMamps, RMamps
ICoeff = 20000 # 377uA/A, over 2k2, => 0.894 V/A
LMamps = Lsens.read_u16() / ICoeff
RMamps = Rsens.read_u16() / ICoeff
################################################################################
# Motor Control LM18200 H-bridge
PWMfreq = 50000
LeftDIR = Pin(15, Pin.OUT, value = 0)
LeftBRK = Pin(25, Pin.OUT, value = 1)
LeftPWM = PWM(Pin(27))
LeftPWM.freq(PWMfreq)
LeftPWM.duty_u16(0)
RightDIR = Pin(14, Pin.OUT, value = 0)
RightBRK = Pin(24, Pin.OUT, value = 1)
RightPWM = PWM(Pin(26))
RightPWM.freq(PWMfreq)
RightPWM.duty_u16(0)
def MotDrive(left = 'off', right = 'off'):
global LeftPower, RightPower
maxPWM = 2**16 - 1
if left == 'off': # driver off, no current
LeftPWM.duty_u16(0)
LeftBRK.value(1)
LeftPower = 0
else:
LD = int(left)
if LD == 0: # active brake: source 1 & source 2
LeftPWM.duty_u16(maxPWM)
LeftBRK.value(1)
LeftDIR.value(0)
LeftPower = 0
elif LD > 0: # going forward:" source 1 & sink 2"
LeftPWM.duty_u16(LD)
LeftBRK.value(0)
LeftDIR.value(1)
LeftPower = LD
else: # going back:" sink 1 & source 2"
LeftPWM.duty_u16(0-LD)
LeftBRK.value(0)
LeftDIR.value(0)
LeftPower = LD
if right == 'off':
RightPWM.duty_u16(0)
RightBRK.value(1)
RightPower = 0
else:
RD = int(right)
if RD == 0:
RightBrake = True
RightPWM.duty_u16(maxPWM)
RightBRK.value(1)
RightDIR.value(0)
RightPower = 0
elif RD > 0:
RightPWM.duty_u16(RD)
RightBRK.value(0)
RightDIR.value(0)
RightPower = RD
else:
RightPWM.duty_u16(0-RD)
RightBRK.value(0)
RightDIR.value(1)
RightPower = RD
MotDrive()
################################################################################
# Steering Servo
# 20ms cycle, pulse 1-2ms, range adj - trial and error to find limits
Scentre = 5300
Slow = 3900 # +/- 30 deg
Shigh = 6200
SteeringServo = PWM(Pin(18))
SteeringServo.freq(50)
SteeringServo.duty_u16(Scentre)
PanServo = PWM(Pin(6))
PanServo.freq(50)
Tlow = 3300
Tcentre = 4900
Thigh = 8000
TiltServo = PWM(Pin(7))
TiltServo.freq(50)
TiltServo.duty_u16(Tcentre)
################################################################################
# Main Loop
dataM = [0] * 6
LeftPower, RightPower = 0, 0
def GetData():
global dataM
dataM[0] = dataV[0]
dataM[1] = dataV[1]
GetMotorI()
dataM[2] = LMamps
dataM[3] = RMamps
dataM[4] = LeftPower
dataM[5] = RightPower
Scheduler = 0
LEDupdate('GreenSolid')
Mode = 'Normal'
while True:
sleep_ms(1)
Scheduler += 1
if (Scheduler - 2) % 50 == 0:
# ch1-6 === dataR[0]-[5]
if Mode == 'RC':
if dataR[0] > 10: # as THR off defaults to about 7%
dd = int(dataR[0] / 100 * 2**16) # ch1 thro: PWM
if dataR[4] < 90: # ch5 gear: direction
MotDrive(dd,dd)
else:
MotDrive(-dd,-dd)
else:
MotDrive(0, 0)
S = (dataR[1]-50) / 50 #ch2 aile: steering
if S > 0.02:
y = Scentre + ((Shigh - Scentre) * S)
elif S < -0.02:
y = Scentre + ((Scentre - Slow) * S)
else:
y = Scentre
SteeringServo.duty_u16(int(y))
T = (dataR[2]-50) / 50 #ch3 elev: camera tilt
if T > 0.05:
y = Tcentre + ((Thigh - Tcentre) * T)
elif T < -0.05:
y = Tcentre + ((Tcentre - Tlow) * T)
else:
y = Tcentre
TiltServo.duty_u16(int(y))
if (Scheduler - 10) % 100 == 0: #100ms 10ms offset
if RecvCommand(Muart):
print('New Command: ', dataCMD)
if (Scheduler - 20) % 200 == 0:
GetData()
SendTelemetry(Muart, dataM, dataR)
print(dataR, dataM)
if dataR[5] > 80:
if Mode == 'Normal':
Mode = 'RC'
LEDupdate('PinkFlash')
else:
if Mode == 'RC':
Mode = 'Normal'
MotDrive()
SteeringServo.duty_u16(Scentre)
TiltServo.duty_u16(Tcentre)
LEDupdate('GreenSolid')
collect()
if (Scheduler - 33) % 250 == 0:
LEDupdate() # aka flash if selected
if (Scheduler - 20) % 1000 == 0: #1000ms 5ms offset
pass
#collect()
- MPU/Comms.py Inter-MPU Comms
################################################################################
# Roverling Mk II - Telemetry & Control inter-MPU Comms.py
dataMOT = [0] * 12
prevTelemString = ' '
dataCMD = [0] * 6
prevCommandString = ' '
def CheckPacket(ss):
# GPS and inter-MPU only use (sentence based)
payload = bytes(ss[1:ss.find('*')], 'utf-8')
checksum = bytes(ss[ss.find('*')+1:ss.find('*')+3], 'utf-8')
csum = 0
for char in payload:
csum ^= char
return bytes("{:02x}".format(csum).upper(), 'ascii') == checksum
def RecvTelemetry(uu):
global dataMOT
global prevTelemString
x = uu.read()
if x is not None:
if x != prevTelemString:
try:
ss = x.decode('UTF-8')
except:
print('Packet decode error')
else:
if CheckPacket(ss):
if ss.find('$TELMM') == 0:
try:
Lvel = float(ss.split(',')[1])
Rvel = float(ss.split(',')[2])
Lcur = float(ss.split(',')[3])
Rcur = float(ss.split(',')[4])
Lpwm = float(ss.split(',')[5])
Rpwm = float(ss.split(',')[6])
Ch1= int(ss.split(',')[7])
Ch2= int(ss.split(',')[8])
Ch3= int(ss.split(',')[9])
Ch4= int(ss.split(',')[10])
Ch5= int(ss.split(',')[11])
Ch6= int(ss.split(',')[12])
except:
print('Data conversion error (Mot/RC)')
else:
dataMOT[0] = Lvel
dataMOT[1] = Rvel
dataMOT[2] = Lcur
dataMOT[3] = Rcur
dataMOT[4] = Lpwm
dataMOT[5] = Rpwm
dataMOT[6] = Ch1
dataMOT[7] = Ch2
dataMOT[8] = Ch3
dataMOT[9] = Ch4
dataMOT[10] = Ch5
dataMOT[11] = Ch6
prevTelemString = x
return True
else:
print('OTHER:', ss)
else:
pass #Telemetry check sum error
return False
def SendTelemetry(uu, dataM, dataR):
ss = ('TELMM' + ',' + str(dataM[0]) + ',' + str(dataM[1]) + ',' + str(dataM[2]) + ','
+ str(dataM[3]) + ',' + str(dataM[4]) + ',' + str(dataM[5]) + ','
+ str(dataR[0]) + ',' + str(dataR[1]) + ',' + str(dataR[2]) + ','
+ str(dataR[3]) + ',' + str(dataR[4]) + ',' + str(dataR[5]) + ',')
payload = bytes(ss, 'utf-8')
checksum = 0
for char in payload:
checksum ^= char
x = (b'$' + payload + b'*' + bytes("{:02x}".format(checksum).upper(), 'ascii')
+ b'\r\n')
uu.write(x)
def RecvCommand(uu):
global dataCMD
global prevCommandString
x = uu.read()
if x is not None:
if x != prevCommandString:
try:
ss = x.decode('UTF-8')
except:
print('Packet decode error cmd')
else:
if CheckPacket(ss):
if ss.find('$CMDMM') == 0:
try:
SecCheck = int(ss.split(',')[1])
OpCode = int(ss.split(',')[2])
int1 = int(ss.split(',')[3])
int2 = int(ss.split(',')[4])
float1 = float(ss.split(',')[5])
float2 = float(ss.split(',')[6])
except:
print('Data conversion error (Command)')
else:
dataCMD[0] = SecCheck
dataCMD[1] = OpCode
dataCMD[2] = int1
dataCMD[3] = int2
dataCMD[4] = float1
dataCMD[5] = float2
prevCommandString = x
return True
else:
print('OTHER:', ss)
else:
pass # Telemetry check sum error
return False
def SendCommand(uu, Cstr): # just bytes passed straight through
ss = ('CMDMM' + ',' + Cstr + ',')
payload = bytes(ss, 'utf-8')
checksum = 0
for char in payload:
checksum ^= char
x = (b'$' + payload + b'*' + bytes("{:02x}".format(checksum).upper(), 'ascii')
+ b'\r\n')
uu.write(x)
- MPU/QuadDec.py Motor Quadrature Decoder (PIO/SM based)
################################################################################
# Roverling Mk II - Quadrature Decoder
# Count transitions using RP2040 PIO based state machine, no CPU cycles and then
# use timer based interrupt to read accumulated counts, aka velocity. - 5ms
# Motor quadrature optical encoder: 100 CPT, 17.83:1, 1783 counts per revolution
from machine import Pin, Timer
from rp2 import asm_pio, StateMachine
from array import array
import time
REncApin = Pin(12, Pin.IN, Pin.PULL_UP)
REncBpin = Pin(13, Pin.IN, Pin.PULL_UP)
LEncApin = Pin(10, Pin.IN, Pin.PULL_UP)
LEncBpin = Pin(11, Pin.IN, Pin.PULL_UP)
# Initis
PrevREnc = 0
PrevLEnc = 0
dataV = array('f', [0]*2)
# Parameters
EncoderSamplePeriod = 50
Counts2Vel = 400
@asm_pio()
def encoder():
# adapted from https://github.com/adamgreen/QuadratureDecoder
# In C/C++ can define base, which needs to be 0 for jump table to work
# in python not working, but padding out to exactly 32 instructions fixes it
# 16 element jump table based on 4-bit encoder last state and current state.
jmp('delta0') # 00-00
jmp('delta0') # 00-01
jmp('delta0') # 00-10
jmp('delta0') # 00-11
jmp('plus1') # 01-00
jmp('delta0') # 01-01
jmp('delta0') # 01-10
jmp('minus1') # 01-11
jmp('minus1') # 10-00
jmp('delta0') # 10-01
jmp('delta0') # 10-10
jmp('plus1') # 10-11
jmp('delta0') # 11-00
jmp('delta0') # 11-01
jmp('delta0') # 11-10
jmp('delta0') # 11-11
label('delta0') # Program actually starts here.
mov(isr, null) # Make sure that the input shift register is cleared when table jumps to delta0.
in_(y, 2) # Upper 2-bits of address are formed from previous encoder pin readings Y -> ISR[3,2]
mov(y, pins) # Lower 2-bits of address are formed from current encoder pin readings. PINS -> Y
in_(y, 2) # Y -> ISR[1,0]
mov(pc, isr) # Jump into jump table which will then jump to delta0, minus1, or plus1 labels.
label('minus1')
jmp(x_dec,'output') # Decrement x
jmp('output')
label('plus1')
mov(x, invert(x)) # Increment x by calculating x=~(~x - 1)
jmp(x_dec,'next2')
label('next2')
mov(x, invert(x))
label('output')
mov(isr, x) #Push out updated counter.
push(noblock)
jmp('delta0')
nop() #need to pad out to exactly 32 instructions
nop()
nop()
RightMotorEncoder = StateMachine(0, encoder, freq=1000000, in_base=REncApin)
LeftMotorEncoder = StateMachine(1, encoder, freq=1000000, in_base=LEncApin)
def twos_comp(val, bits):
if (val & (1 << (bits - 1))) != 0: # if sign bit is set e.g., 8bit: 128-255
val = val - (1 << bits) # compute negative value
return val # return positive value as is
def QueryEncoders():
global PrevLEnc, PrevREnc
global LeftVel, RightVel
k = 0
while (LeftMotorEncoder.rx_fifo() > 0) and (k < 4): # empty FIFO - last value used
k += 1
LEE = LeftMotorEncoder.get()
if k > 0:
LEE = twos_comp(LEE, 32)
LE = LEE - PrevLEnc
PrevLEnc = LEE
else:
LE = 0
k = 0
while (RightMotorEncoder.rx_fifo() > 0) and (k < 4):
k += 1
REE = RightMotorEncoder.get()
if k > 0:
REE = twos_comp(REE, 32)
RE = REE - PrevREnc
PrevREnc = REE
else:
RE = 0
dataV[0], dataV[1] = LE/ Counts2Vel, RE/Counts2Vel
def cbEncSampleTimer(t):
QueryEncoders()
RightMotorEncoder.active(1)
LeftMotorEncoder.active(1)
EncSampleTimer = Timer(period=EncoderSamplePeriod , mode=Timer.PERIODIC, callback=cbEncSampleTimer)
- MPU/RCinterface.py 6 Channel RC Interface (interrupt based)
################################################################################
# Roverling Mk II - RCinterface.py RC Interface (mine: Spektrum AR6200 Rx, DX6i Tx)
# Interrupt on both edges to mark tick_us points, process later at a reduced
# rate, as required, to keep these IRQ routines as short as possible
from machine import Pin, Timer
from time import ticks_us
from array import array
# Parameters
RCSamplePeriod = 100
ch1 = Pin(5,Pin.IN)
ch2 = Pin(4,Pin.IN)
ch3 = Pin(3,Pin.IN)
ch4 = Pin(2,Pin.IN)
ch5 = Pin(1,Pin.IN)
ch6 = Pin(0,Pin.IN)
dataR = array('B',[0]*6)
tt = ticks_us()
CHtimes = array('H',[tt]*12)
def cbIntCh1(ch1):
global CHtimes
if ch1.value() == 1:
CHtimes[0] = ticks_us()
else:
CHtimes[1] = ticks_us()
def cbIntCh2(ch2):
global CHtimes
if ch2.value() == 1:
CHtimes[2] = ticks_us()
else:
CHtimes[3] = ticks_us()
def cbIntCh3(ch3):
global CHtimes
if ch3.value() == 1:
CHtimes[4] = ticks_us()
else:
CHtimes[5] = ticks_us()
def cbIntCh4(ch4):
global CHtimes
if ch4.value() == 1:
CHtimes[6] = ticks_us()
else:
CHtimes[7] = ticks_us()
def cbIntCh5(ch5):
global CHtimes
if ch5.value() == 1:
CHtimes[8] = ticks_us()
else:
CHtimes[9] = ticks_us()
def cbIntCh6(ch6):
global CHtimes
if ch6.value() == 1:
CHtimes[10] = ticks_us()
else:
CHtimes[11] = ticks_us()
ch1.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=cbIntCh1)
ch2.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=cbIntCh2)
ch3.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=cbIntCh3)
ch4.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=cbIntCh4)
ch5.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=cbIntCh5)
ch6.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=cbIntCh6)
RCavg = [[0,0,0,0,0], [0,0,0,0,0], [0,0,0,0,0], [0,0,0,0,0], [0,0,0,0,0], [0,0,0,0,0]]
def UpdateRC():
global dataR
global RCavg
for i in range(6):
diff = CHtimes[2*i+1] - CHtimes[2*i]
if diff < 2500 and diff > 800:
NewVal = int(min(max((diff - 1000) / 10, 0), 100))
RCavg[i].append(NewVal)
RCavg[i].pop(0) # use median of last 5 samples
tmpList = []
tmpList.extend(RCavg[i])
tmpList.sort()
dataR[i] = tmpList[2] # centre position implies 2 values above and 2 below
def cbRCSampleTimer(t):
UpdateRC()
rcSampleTimer = Timer(period=RCSamplePeriod , mode=Timer.PERIODIC, callback=cbRCSampleTimer)
Telemetry Processor
Aggregates ALL data from the platform for telemetry transmission over LoRa at around 1800 bps. Accepts LoRa commands. Telemetry includes:
- GPS: quality, number satellites, accuracy, altitude, lat & long with 7 decimal places precision
- IMU: acceleration (X, Y, Z), gyroscope (X, Y, Z), magnetometer (X, Y, Z), temperature, pressure
- Locomotion: velocity (L/R), torque/current (L/R), power delivered (L/R)
- RC: 6 channels, 0 to 100%
- Platform: PIR, sonar range, battery level, status bits
- Comms: RSSI & SNR for both Roverling and Base Station ends
- MPU/mainT.py
################################################################################
# Roverling Mk II - Telemetry Processor
print('Roverling Mk II - Telemetry Processor main.py on RP2040')
from machine import Pin, I2C, UART, ADC
from time import sleep_ms, sleep_us, ticks_ms, ticks_us
from gc import collect
import neopixel
from cryptolib import aes
# my modules
from GPS import ReadGPS, dataGPS
from IMU import ReadIMU, dataIMU
from Packets import AssemPkt_T, DisAssemPkt_T, AssemPkt_C, DisAssemPkt_C
from Comms import RecvTelemetry, dataMOT, SendCommand
Tuart = UART(1, baudrate=1_000_000, tx=Pin(4), rx=Pin(5))
################################################################################
# LED
NeoPin = Pin(16,Pin.OUT)
Neo = neopixel.NeoPixel(NeoPin,1)
LastLEDstate = True
RGBon, RGBoff = (20,0,0), (0,0,0)
Neo[0]=(20, 0, 0)
neopixel.NeoPixel.write(Neo)
def LEDupdate(mode='none'):
global Neo, LastLEDstate, RGBon, RGBoff
if mode == 'GreenSolid':
RGBon, RGBoff = (20,0,0), (20,0,0)
elif mode == 'RedSolid':
RGBon, RGBoff = (0,40,0), (0,40,0)
elif mode == 'YellowFlash':
RGBon, RGBoff = (6,20,0), (0,0,0)
elif mode == 'PinkFlash':
RGBon, RGBoff = (5,50,10), (0,0,0)
elif mode == 'BlueSolid':
RGBon, RGBoff = (0,0,30), (0,0,30)
if not LastLEDstate:
Neo[0] = RGBon
LastLEDstate = True
else:
Neo[0] = RGBoff
LastLEDstate = False
neopixel.NeoPixel.write(Neo)
LEDupdate('RedSolid')
################################################################################
# LoRa
from sx1262 import SX1262
sx = SX1262(spi_bus=1, clk=10, mosi=11, miso=12, cs=3, irq=6, rst=7, gpio=2)
# Time On Air : Telemetry 64 bits, 195ms, Control 16 bits, 82ms
# At BW=250kHz, SF=9, CR=1/5, Date Rate = 1,758bps
# for max power current limit must be changed for 60 - 140 and LDO enabled
#
sx.begin(freq=920, bw=250.0, sf=9, cr=5, syncWord=0x12,
power=22, currentLimit=140.0, preambleLength=8,
implicit=False, implicitLen=0xFF,
crcOn=True, txIq=False, rxIq=False,
tcxoVoltage=1.7, useRegulatorLDO=True, blocking=True)
NewCmd = False
NewCmdPkt = [0x00] * 16
SNR, RSSI = 0.0, 0.0
def cbLoRa(events):
global NewCmd
global NewCmdPkt
global SNR
global RSSI
if events & SX1262.RX_DONE:
msg, err = sx.recv()
error = SX1262.STATUS[err]
if err == 0:
NewCmd = True
NewCmdPkt = msg
SNR = sx.getSNR()
RSSI = sx.getRSSI()
else:
print('LoRa Rx Error: ', error)
elif events & SX1262.TX_DONE:
pass #TX done
else:
print('LoRa unknown interrupt: ', events)
VBatList = [0] * 9
################################################################################
# Battery Voltage 5cell LiPo system (https://blog.ampow.com/lipo-voltage-chart/)
Vbat = ADC(Pin(29))
def GetVbat():
global VbatList
VbatCoeff = 3050 # through testing
V = Vbat.read_u16() / VbatCoeff / 5 # 5 cells, nom 18V
if V >= 4.20: cap = 100
elif V >= 4.15: cap = 95
elif V >= 4.11: cap = 90
elif V >= 4.08: cap = 85
elif V >= 4.02: cap = 80
elif V >= 3.98: cap = 75
elif V >= 3.95: cap = 70
elif V >= 3.91: cap = 65
elif V >= 3.87: cap = 60
elif V >= 3.85: cap = 55
elif V >= 3.84: cap = 50
elif V >= 3.82: cap = 45
elif V >= 3.80: cap = 40
elif V >= 3.79: cap = 35
elif V >= 3.77: cap = 30
elif V >= 3.75: cap = 25
elif V >= 3.73: cap = 20
elif V >= 3.71: cap = 15
elif V >= 3.69: cap = 10
elif V >= 3.61: cap = 5
else: cap = 0
# get median over 9 samples
VBatList.append(cap)
VBatList.pop(0 )
tmpList = []
tmpList.extend(VBatList)
tmpList.sort()
return(tmpList[4])
################################################################################
Vsonar = ADC(Pin(27))
def GetSonar():
global Sonar
VsonarCoeff = 9.0
S = Vsonar.read_u16() / VsonarCoeff
return int(S)
Vpir = Pin(28, Pin.IN, Pin.PULL_UP)
def GetPIR():
if Vpir.value() == 1:
return True
else:
return False
def set_bit(value, bit):
return value | (1<
- MPU/GPS.py Configure & Read GPS Module Data
################################################################################
# Roverling Mk II - GPS.py GPS module
#
# 03/09/23 Not enough precision in 32 bit float for calculation of decimal
# degrees to better than 8m longitudely. Will fix by
# and transporting only minutes and hard coding degrees at Rx end.
# 25/08/23 ran out of UARTS,reconfiguring to use PIO based UART instead
# wish I has an extra real one, this has been problematic
# byte assembly interrupt is being interrupted
# 22/10/23 back to HW based UART - nothing but problems with PIO based
# implementation. Now out of UARTs so will probably need to fix so
# we can talk to RPI4
#from PIO_UART import PIO_UART0_readline, PIO_UART0_write
from machine import Pin, UART
from Comms import CheckPacket
Guart = UART(0, baudrate=9600, tx=Pin(0), rx=Pin(1), timeout=10 )
dataGPS = ['000000.000', 0.0, 0.0, 0, 0, 0, 0]
prevString = ' '
def ReadGPS():
global dataGPS
global prevString
#x = PIO_UART0_readline()
x = Guart.readline()
if not x is None:
if x != prevString:
ss = ''.join(chr(i) for i in x)
if CheckPacket(ss):
if ss.find('$GNGGA') == 0: #maybe for GPGGA as well?
try:
Gutc = (ss.split(',')[1])
s = ss.split(',')[2]
Glattide = float(s[2:9])/60
s = ss.split(',')[4]
Glongitude = float(s[3:10])/60
Gquality = int(ss.split(',')[6])
Gnumstats = int(ss.split(',')[7])
Gaccuracy = float(ss.split(',')[8])
Galtitude = float(ss.split(',')[9])
except:
print('ERROR data conversion error (GPS)')
else:
dataGPS[0] = Gutc
dataGPS[1] = Glattide
dataGPS[2] = Glongitude
dataGPS[3] = Gquality
dataGPS[4] = Gnumstats
dataGPS[5] = Gaccuracy
dataGPS[6] = Galtitude
prevString = x
return True
else:
print('ERROR, unexpected response:', ss)
else:
print('ERROR failed CheckPacket:',ss)
else:
print('ERROR duplicate packet:')
return False
def WriteGPS(command: bytes):
checksum = 0
for char in command:
checksum ^= char
x = b'$' + command + b'*' + bytes("{:02x}".format(checksum).upper(), 'ascii') + b'\r\n'
#PIO_UART0_write(x.decode('UTF-8'))
Guart.write(x.decode('UTF-8'))
# Configure GPS device MTK3339
# https://cdn-shop.adafruit.com/datasheets/PMTK_A08.pdf (NOT v11 !)
# Update rate and position fix to 1000 max for SBAS, else max is 200ms
WriteGPS(b'PMTK220,1000')
WriteGPS(b'PMTK300,1000,0,0,0,0')
# Enable SBAS augmentation: ! only if output rate < 1Hz
WriteGPS(b'PMTK313,1')
# Turn on GGA only
WriteGPS(b"PMTK314,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0")
# Expect in return, 4x $PMTK001, with flag set to 3
- MPU/IMU.py Inertial Measurement Unit Interface
################################################################################
# Roverling Mk II - Interface for AltIMU-10 v6 carrier:
# LSM6DSO Accelerometer/Gyro/Temp
# LPS22DF Barometer
# LIS3MDL Magnetometer
from machine import Pin, I2C
import struct
i2c = I2C(1, sda=Pin(14), scl=Pin(15), freq=100_000)
def DevWrite(addr, reg, data):
i2c.writeto_mem(addr, reg, data.to_bytes(1, 'little'))
def DevReadInt16(addr, regLSB):
lsb, msb = i2c.readfrom_mem(addr, regLSB, 2)
return struct.unpack('
- MPU/Comms.py Inter-MPU Comms
- MPU/Packets.py Telemetry & Control LoRa Packet Assembly & Disassembly
################################################################################
# Roverling Mk II - Packets.py
# Packet assembly / disassembly / compression / encryption
# Need to pack telemetry data as tight as possible. Most fields will fit into
# 2 bytes. This has reduced struct.pack size from 106 bytes to 58 (now 64)
# also encrypting so I'm not advertising my exact location to the world
import struct
from cryptolib import aes
char48 = b'1234567890oplmjuyhgliughuxcdhgfjsddsfgreatgadefg'
PktFmt_T = 'HffBHH6h6b9h2H6B' # 64 bytes
PktFmt_C = '4H2f'
from Secrets import key_T, key_C
def AssemPkt_T(dataGPS, dataMOT, dataIMU, dataOther):
# GPS time - now seconds past the hour (unsigned int16)
SecondsPastHour = (int(dataGPS[0][2:4])*60) + (int(dataGPS[0][4:6])*1)
# Decimal portion only of degrees
Latitude, Longitude = dataGPS[1],dataGPS[2]
# encode quality in top 3 bits and number of sats in bottom 5 bits (byte)
QualSats = (dataGPS[3] << 5) + dataGPS[4]
# accuracy, in decimeters (unsigned int16)
Accuracy = int( dataGPS[5] * 10)
# altitude, in decimeters (unsigned int16)
Altitude = int( dataGPS[6] * 10)
# left / right velocity in mm/sec (signed int16)
LeftVel, RightVel = int(dataMOT[0] * 1000) ,int(dataMOT[1] * 1000)
# l / r motor current in mA (unsigned int16)
LeftCur, RightCur = int(dataMOT[2] * 1000) ,int(dataMOT[3] * 1000)
# l / r PWM, divided by two so we can add sign for direction (signed int16)
LeftPWM, RightPWM = int(dataMOT[4] / 2) ,int(dataMOT[5] / 2)
# RC channels (byte)
# dataMOT[6..11]
# Accel in mg (signed int16)
AccelX, AccelY, AccelZ = int(dataIMU[0]*1000),int(dataIMU[1]*1000),int(dataIMU[2]*1000)
# Gyro in dps (signed int16)
GyrolX, GyroY, GyroZ = int(dataIMU[3]),int(dataIMU[4]),int(dataIMU[5])
# Magnetometer in mG (signed int16)
MagX, MagY, MagZ = int(dataIMU[6]*1000),int(dataIMU[7]*1000),int(dataIMU[8]*1000)
# temp mill deg celcius (unsigned int16)
Temp = int(dataIMU[9]*1000)
# Pressure =hPa * 10 (unsigned int16)
Pressure = int(dataIMU[10]*10)
# Last Received SNR dB (char) -30 -> +10 scale
SNR = int((dataOther[0]+ 30) * 4)
# Last RSSI dBm (char) -130 -> -30 scale
RSSI = int((dataOther[1]+ 130) * 2)
# Sonar distance 200mm - 7650mm (8192 -> 256, char)
Sonar = int(dataOther[2] / 32)
# BatPercent % (char)
BatPercent = int(dataOther[3])
# Bits * 16
# dataOther[4..5]
ss = (struct.pack(PktFmt_T, SecondsPastHour, Latitude, Longitude, QualSats,
Accuracy, Altitude, LeftVel, RightVel, LeftCur, RightCur, LeftPWM, RightPWM,
dataMOT[6],dataMOT[7],dataMOT[8],dataMOT[9],dataMOT[10],dataMOT[11],
AccelX, AccelY, AccelZ, GyrolX, GyroY, GyroZ, MagX, MagY, MagZ, Temp, Pressure,
SNR, RSSI, Sonar, BatPercent, dataOther[4], dataOther[5] ))
cipher = aes(key_T, 1)
ee = cipher.encrypt(ss)
return ee
def DisAssemPkt_T(pkt):
cipher = aes(key_T, 1)
try:
ee = cipher.decrypt(pkt)
except:
return 'Decryption Error'
else:
pk = struct.unpack(PktFmt_T, ee)
SecondsPastHour = pk[0]
LatDegrees, LongDegrees = -37, 144
LatMinutes, LongMinutes = pk[1],pk[2]
Quality = (pk[3] & 0xe0) >> 5
NumSats = pk[3] & 0x1f
Accuracy = pk[4] / 10
Altitude = pk[5] / 10
LeftVel, RightVel = pk[6] / 1000, pk[7] / 1000
LeftCur, RightCur = pk[8] / 1000, pk[9] / 1000
LeftPWM, RightPWM = pk[10] * 2, pk[11] * 2
ch1,ch2,ch3,ch4,ch5,ch6 = pk[12],pk[13],pk[14],pk[15],pk[16],pk[17],
AccelX, AccelY, AccelZ = pk[18] / 1000, pk[19] / 1000, pk[20] / 1000
GyrolX, GyroY, GyroZ = pk[21],pk[22],pk[23]
MagX, MagY, MagZ = pk[24] / 1000, pk[25] / 1000, pk[26] / 1000
Temp = pk[27] / 1000
Pressure = pk[28] / 10
SNR = (pk[29] / 4) - 30
RSSI = (pk[30] / 2) - 130
Sonar = pk[31] * 32
BatPercent = pk[32]
B1, B2 = pk[33], pk[34]
tt = (SecondsPastHour,
str(LatDegrees)+str(LatMinutes)[1:]+', '+ str(LongDegrees)+str(LongMinutes)[1:],
Quality, NumSats, Accuracy, Altitude,
LeftVel, RightVel,LeftCur, RightCur,LeftPWM, RightPWM,ch1,ch2,ch3,ch4,ch5,ch6,
AccelX, AccelY, AccelZ, GyrolX, GyroY, GyroZ, MagX, MagY, MagZ, Temp, Pressure,
SNR, RSSI, Sonar, BatPercent, B1, B2)
return(tt)
def AssemPkt_C(SecCheck, OpCode, int1, int2, float1, float2):
ss = struct.pack(PktFmt_C, SecCheck, OpCode, int1, int2, float1, float2)
cipher = aes(key_C, 1)
ee = cipher.encrypt(ss)
return ee
def DisAssemPkt_C(pkt):
cipher = aes(key_C, 1)
try:
ee = cipher.decrypt(pkt)
except:
return 'Decryption Error'
else:
pk = struct.unpack(PktFmt_C, ee)
SecCheck = pk[0]
OpCode = pk[1]
int1, int2 = pk[2], pk[3]
float1, float2 = pk[4], pk[5]
values = SecCheck, OpCode, int1, int2, float1, float2
string = (str(pk[0]) + ',' + str(pk[1]) + ',' + str(pk[2]) + ',' +
str(pk[3]) + ',' + str(pk[4]) + ',' + str(pk[5]))
return(values, string)
- MPU/Secrets.py Modify and rename Secret_Example.py to Secrets.py
LoRa drivers from https://github.com/ehong-tl/micropySX126X/tree/master - MPU/lib/_sx126x.py
- MPU/lib/sx1262.py
- MPU/lib/sx126x.py
Gateway Processor
Provides a gateway for Roverling LoRa commands and telemetry to MQTT message broker over WiFi. The onboard LED flashes blue when commands are received. A single message is delivered twice a second and looks a lot like this. (On MQTT Explorer): (3172, '-37.0000000, 144.0000000', 1, 4, 3.0, 640.4, 0.0, 0.0, 0.039, 0.043, 0, 0, 10, 50, 51, 51, 9, 10, 0.013, -0.037, 1.015, 0, 0, 0, 0.034, -0.037, 0.455, 19.734, 944.5, 10.25, -42.0, 384, 0, 0, 0, 11.0, -45.0)
################################################################################
# Roverling Mk II - Telemetry Gateway
print('Roverling Mk II - Gateway main.py on RP2 Pico W')
from time import sleep_ms
import network
from umqttsimple import MQTTClient
from sx1262 import SX1262
from Packets import AssemPkt_T, DisAssemPkt_T, AssemPkt_C, DisAssemPkt_C
# LoRa
sx = SX1262(spi_bus=1, clk=10, mosi=11, miso=12, cs=3, irq=20, rst=15, gpio=2)
sx.begin(freq=920, bw=250.0, sf=9, cr=5, syncWord=0x12,
power=22, currentLimit=140.0, preambleLength=8,
implicit=False, implicitLen=0xFF,
crcOn=True, txIq=False, rxIq=False,
tcxoVoltage=1.7, useRegulatorLDO=True, blocking=True)
# WiFi
from Secrets import wifi_ssid, wifi_password
wlan = network.WLAN(network.STA_IF)
wlan.active(True)
wlan.connect(wifi_ssid, wifi_password)
while wlan.isconnected() == False:
print('Waiting for connection...')
sleep_ms(1000)
print("Connected to WiFi")
# MQTT
MQTTClientID = 'RV01'
MQTTBroker = '192.168.2.100'
from Secrets import MQTTUser, MQTTPassword
TopicBase = 'DEV/RV/'
def Publish(topic, mess):
try:
client.publish(TopicBase + topic, mess, False)
except:
print('MQTT Pub fail')
client.connect()
def MQTT_cb(topic, msg):
print(topic, msg)
client = MQTTClient(MQTTClientID, MQTTBroker, user=MQTTUser, password=MQTTPassword)
client.connect()
client.set_callback(MQTT_cb)
client.subscribe('DEV/RV/cmd')
while True:
msg, err = sx.recv()
snr = sx.getSNR()
rssi = sx.getRSSI()
#print('SNR:',snr,' RSSI:', rssi, 'PKT: ', end='')
if len(msg) > 0:
if err != 0:
error = SX1262.STATUS[err]
print('LoRa Recv Error ',error)
else:
y = DisAssemPkt_T(msg)
x = str(y) + ',' + str(snr) + ',' + str(rssi)
Publish('Packet', x)
print(x)
'''print(y)
Publish('Position', y[1])
Publish('Quality', str(y[2]))
Publish('NumSats', str(y[3]))
Publish('Accuracy', str(y[4]))
Publish('Altitude', str(y[5]))
Publish('LeftVel', str(y[6]))
Publish('RightVel', str(y[7]))
Publish('LeftCur', str(y[8]))
Publish('RightCur', str(y[9]))
Publish('LeftPWM ', str(y[10]))
Publish('RightPWM', str(y[11]))
Publish('ch1', str(y[12]))
Publish('ch2', str(y[13]))
Publish('ch3', str(y[14]))
Publish('ch4', str(y[15]))
Publish('ch5', str(y[16]))
Publish('ch6,', str(y[17]))
Publish('AccelX', str(y[18]))
Publish('AccelY', str(y[19]))
Publish('AccelZ', str(y[20]))
Publish('GyrolX', str(y[21]))
Publish('GyroY', str(y[22]))
Publish('GyroZ', str(y[23]))
Publish('MagX', str(y[24]))
Publish('MagY', str(y[25]))
Publish('MagZ', str(y[26]))
Publish('Temp', str(y[27]))
Publish('Pressure', str(y[28]))
Publish('RoverSNR', str(y[29]))
Publish('RoverRSSI', str(y[30]))
Publish('Sonar', str(y[31]))
Publish('BatPercent', str(y[32]))
Publish('GatewaySNR', str(snr))
Publish('GatewayRSSI', str(rssi))
Publish('B1 - PIR', str(y[33]))
Publish('B2', str(y[34]))
'''
try:
client.check_msg()
except:
print('MQTT Check fail')
client.connect()
sleep_ms(100)
x = AssemPkt_C(123, 0x02, 4, 44, 1.2, 1.3)
sx.send(x)
- MPU/Packets.py telemetry and control LoRa packet assembly and disassembly
- MPU/Secrets.py modify and rename Secret_Example.py to Secrets.py
LoRa drivers from https://github.com/ehong-tl/micropySX126X/tree/master
- MPU/lib/_sx126x.py
- MPU/lib/sx1262.py
- MPU/lib/sx126x.py
- MPU/lib/umqttsimple.py MQTT module from https://pypi.org/project/micropython-umqtt.simple/#files
Gateway Board
Notes on powering
Roverling was designed so that code development can take place without providing any external power. The USB-C connection to either processor for download, test, and dev, is enough to power everything on board, except motors, including the RPi4, surprisingly.
I use this all the time and use the onboard power switch to fully power when I want to drive the motors. This system has allowed for very rapid development. The GUI will show a dangerously low battery voltage, just ignore this.
Tele18.py and tele18.ui are the files you need, available here. I use VS Code to develop and run these modules in a venv. Sorry, I haven’t properly packaged anything.
~/python -m venv env
~/source env/bin/activate
pip install pyside6
pip installpaho-mqtt
Telemetry and command data to and from the Roverling is achieved through MQTT.
Roverling < — (LoRa 1800 bps, twice per second) — > Gateway
Gateway < — (MQTT over TCP via WIFi) — > Desktop Computer (GUI)
You need to have Wi-Fi and MQTT broker infrastructure already in place to use this solution. You could also remix the gateway to provide any other kind of communication channel/protocol. You can also use any MQTT-enabled device. I use an app called IOT ON/OFF on my iPad and iPhone and use MQTT Explorer on my Linux desktop.
I use Mosquitto, an open-source MQTT broker, that runs on an RPi4 which also provides all the network services I need around here.
It’s all coded in Python using the Qt framework, on Xubuntu, I installed using: pip install pyside6.
The GUI graphics were designed in using pyside6-designer and compiled into Python with pyside6-uic.
The black widget in the bottom right of the GUI is reserved for video streaming but has not been coded yet.
MQTT Explorer worked great for development. In the below screenshot, each parameter was conveyed by a different MQTT topic. This proved problematic in the Qt / MQTT framework, so now there is a single topic that returns a string with ALL parameters.
GUI Elements
COMMS
Click on RUNNING/STOP to process MQTT packets
Pkt count shows the number of packets received
GPS Sec shows GPS minutes/seconds past the hour
SNR and RSSI displayed for both ends
SYSTEM
Shows the approximate battery charge level, PIR status, sonar range, and 16 status bits.
RECORD
Click Paused / Recording to toggle a recording. Very simply, all received packets are saved to a CSV file that can be played back as if telemetry is being received live, except that you can change the speed.
LOCOMOTION
Displays velocity (not yet calibrated), current (same as torque and will be used to detect slip or stall in the future), and applied power. At the moment power is controlled only by RC channel 1. Soon I’ll write a proper PID controller with velocity and max acceleration as the input parameters. Negative velocity is shown when reversing.
IMU
Show processed data.
Heading based on X & Y magnetometer
Pitch and roll based on X, Y, and Z components of the accelerometer
Temp, pressure, and calculated altitude based on pressure & temp
X & Y magnetic flux components, current limits, and new ranges for calibration
This picture shows the result of slow rotation about the centre, followed by a quick direction change. X and Y axes represent the respective magnetic fields.
Magnetometer calibration (see here)
The first few seconds of the video show the X & Y components of the Earth's magnetic field as the Roverling is slowly rotated around on an office chair. It's pretty round but has a little distortion due to the magnet in the GPS antenna which is about 150mm away.
The last half of the video shows the fields whilst the Roverling is outside moving around, as per the previous post. I carefully twisted all current-carrying wires with their return to prevent EMI from impacting the magnetometer additionally, the Roverling was moved manually rather than on its own power, so there shouldn't be too much EMI generated due to motor currents. The chart clearly shows field strength almost doubling in some locations. There is a big spike as I travel around a power pole and others that I'm unsure about. My calibration is good enough for now and could probably be further enhanced using GPS positional deltas to calculate heading.
Also used this - sometimes the old ways are the best.
GPS
Pretty much everything out of the $GPGGA sentence.
Below are the tools to set the origin of the aerial photo. You’ll need to calibrate. See this instagram video. You’ll need to modify the code, put in record mode, and drive around, as much as possible visiting accurate positions you can see on the map. Then adjust the bottom left coordinates and scaling factor.
Yellow points on the graph indicate that quality = 1, and red means we had quality = 2
You may also notice the GPS variance chart. This was created to observe variance/error when NOT moving. It really needs to sit outside in the open sky for about 5-10 minutes, and at that point, the divergence is usually less than 30cm. Interestingly the GPS quality number (1 or 2) or the reported accuracy doesn't appear to have any correlation with the actual accuracy/divergence that I measure. What I found more important is the number of satellites in view. I found at my location I need at least 15+ to achieve a variance of less than 0.5m, but I usually get 19 and at most 22. For this run, you will notice that I managed 19-20 sats, even under trees.
In the example, the Roverling was inside a building so the variance is significant. About 6m in the latitude or X axis and nearly 10m in the longitude or Y axis. Sit Roverling outside in a clear sky for 5-10 minutes, reset the graph and you shouldn’t observe variance in the range of 0.1 to 0.3 m.
RC
Shows details of all 6 channels. Channel 6 > 75% puts the Locomotion Processor into RC control mode, shown by this panel changing to light blue. Ensure CH1, throttle, is at minimum before toggling gear (ch6) switch - otherwise, Roverling may shoot off the desk! Happened to me more than once whilst developing.
Raspberry Pi 4B 4GB w/ 64 GB SD
Flashed with most recent 32-bit Raspberry Pi OS w/ desktop (3/5/23)
Raspberry Pi Camera Module 3
I played around with object recognition, face recognition, video streaming (UDP & TCP), and video capture and save.
This is all a work in progress but all my tests and examples can be found here.
