Last Updated on 18 september 2022 by Syds
Kwam bij toeval op Instructables.com een leuk DIY project tegen, het maken van een robotstofzuiger. Dus op basis van de beschrijving van Cesar Nieto ben ik aan de slag gegaan. Eerst met een Arduino Uno, maar na vele uren zwoegen op de C++ code kreeg ik dit niet stabiel. Ik wilde natuurlijk nog wat verder gaan dan Cesar, en een app koppelen aan de robotstofzuiger zodat ik via mijn telefoon (of willekeurig ander device):
- Real time informatie kreeg vanuit alle sensoren voor debugging doeleinden
- De bedrijfstijden kon instellen
- Een attentiesignaal krijg als de batterij leeg is
- Een attentiesignaal krijg als de robotstofzuiger zichzelf klem heeft gereden
- Een indicatie krijg tot hoever de batterij opgeladen
- Een attentiesignaal krijg als de batterij vol is
- Een attentiesignaal krijg als de stofzak vol is
Mijn kennis van C++ bleek te kort te schieten om bovenstaande (stabiel) te realiseren, daarom besloten om het met een ESP32 met hierop Micropython geflashed te proberen. En met succes ! Lees verder hoe.
Links
https://www.instructables.com/Build-Your-Own-Vacuum-Robot/
Materiaallijst
Artikel | Aantal | Kosten per stuk | Totale kosten | Te bestellen bij: |
ESP32 WROOM Devkit v1 4Mb | 1 | 10,28 | 10,28 | Otronic |
IRF520 MOS FET Driver Module | 1 | 1,45 | 1,45 | Aliexpress |
H-bridge L298 Dual Motor Driver | 1 | 2,05 | 2,05 | Aliexpress |
Gearmotor DC 12V 101RPM + Wheel Kit | 2 | 4,61 | 12,05 | Aliexpress |
Fan blower AVC BA10033B12G DC 12V 4.5A | 1 | 14,30 | 14,30 | Aliexpress |
Sharp Distance Sensor GP2Y0A41SK0F | 4 | 3,09 | 12,36 | Aliexpress |
ZIPPY Compact 1300mAh 3S 25C Lipo Pack | 1 | 10,29 | 10,29 | HobbyKing |
XT-60H-M male stekker | 1 | 0,67 | 0,67 | TME |
2k Ohm potentiometer | 1 | 2,28 | 2,28 | Aliexpress |
M3 schroeven + boutjes | 20 | 4,13 | Aliexpress | |
Ball caster metal ball | 1 | 1,77 | 1,77 | Aliexpress |
Pushbuttons 25 stuks | 2 | 1,56 | 1,56 | Aliexpress |
#8-32 x 2 inch bouten + moeren + ring | 2 | 0,11 | 0,22 | Plat |
Vacuum bag filter (cloth type) | 1 | uit WTW filter | ||
On/Off button switch red 12v momentary | 1 | 1,04 | 1,04 | Aliexpress |
Socket voor button | 1 | 0,62 | 0,62 | Aliexpress |
Rood/zwarte draad 5m | 1 | 2,57 | 2,57 | Aliexpress |
LiPo Battery Charger 3s | 1 | 6,83 | 6,83 | Aliexpress |
Loctite Glue-3 5g | 1 | 5,77 | 5,77 | Praxis |
Schroef connectors 2 pin 10-stuks | 1 | 0,65 | 0,65 | Aliexpress |
Jumper cables 30cm male-female | 1 | 1,82 | 1,82 | Aliexpress |
5.5 Mm X 2.1 Mm 5.5X2.1 Dc Voeding Plug | 1 | 0,47 | 0,47 | Aliexpress |
Stepdown buck converter | 1 | 0,91 | 0,91 | Aliexpress |
PLA filament 1,75mm 1 kg | 1 | 19,50 | 19,50 | 123Inkt.nl |
Totaal | 105,45 |
Pinout
ESP32 | 1e component | 2e component |
H-Bridge | MOS Fet driver | |
VCC +12v | VIN | |
GND | GND | |
D12 | SIG | |
GND | GND | |
VIN | 5V | |
GPIO19 | IN1 | |
GPIO21 | IN2 | |
GPIO22 | IN3 | |
GPIO23 | IN4 | |
Sharp Sensor links | ||
GPIO36 | gele draad | |
VIN | rode draad | |
GND | Zwarte draad | |
Sharp Sensor Links zij | ||
GPIO35 | gele draad | |
VIN | rode draad | |
GND | zwarte draad | |
Sharp Sensor rechts | ||
GPIO34 | gele draad | |
VIN | rode draad | |
GND | zwarte draad | |
Sharp Sensor rechts zij | ||
GPIO39 | gele draad | |
VIN | rode draad | |
GND | zwarte draad | |
LED | ||
GPIO16 | lange poot (Anode) | |
GND | korte poot (Kathode) | |
Bumper links | ||
GPIO17 | Rode draad | |
GND | Zwarte draad | |
Bumper rechts | ||
GPIO18 | Rode draad | |
GND | Zwarte draad | |
Batterij indicatie | ||
GPIO32 | Witte draad (vanaf potmeter) | |
GND | GND | |
Stap 1. ESP32 geflashed met MicroPython
Lees hiervoor mijn eerdere blog Micropython flashen op een ESP32
Stap 2. Zaag en boor mal gemaakt voor prototype
Had nog geen 3D-printer, dus ik heb eerst een werkend prototype gemaakt zonder stofzuigergedeelte m.b.v. een kunststof rattan mandje van de Action. Voor het uitzagen en gaatjes boren t.b.v. het bevestigen van de wielen en draaibal heb ik een zaagmal gemaakt
- Deze zaagmal heb ik met tape aan de onderkant van het postvakje bevestigd, vervolgens de gaatjes geboord en uitsparingen voor de wielen (voor de draaibal is er geen uitsparing nodig) uitgezaagd met een figuurzaag.
Stap 3. Wieltjes en draaibal gemonteerd
- Een stukje snoer op de puntjes van de motoren gesoldeerd en de andere kant van het snoer op de out2 en out3 aansluitingen van de H-bridge geschroefd. (Polariteit maakt niet uit, dit kun je evt. softwarematig aanpassen.)
- De wieltjes op de motoren geklikt en met de mee bestelde beugeltjes incl. boutjes en moertjes bevestigd in het mandje
- Tevens de draaibal met een paar bouten en moertjes bevestigd aan de onderkant van het mandje. Grappig detail is dat de H-bridge precies over de uiteinden van de boutjes geschoven kon worden, en dus gelijk mooi vastgezet is.
Stap 4. H-bridge aangesloten op ESP32
- H-bridge als volgt aangesloten
H-Bridge | ESP32 | MOS Fet driver |
VCC +12v | VIN | |
GND | GND | |
5V | VIN | |
IN1 | GPIO19 | |
IN2 | GPIO21 | |
IN3 | GPIO22 | |
IN4 | GPIO23 |
Stap 5. Led aangesloten
- Led aangesloten op GPIO16 en GND van de ESP32
Stap 6. Sharp sensoren aangesloten
- Voedingsblokje gemaakt van printplaatje met 3 2-voudige schroefterminals
- Sensoren erop aangesloten (Rood = + VCC, Zwart = – GND), en gevoed vanuit 5V en GND van de Arduino
- Data kabels van sensoren op Arduino aangesloten
- male-male dupont kabeltje doormidden geknipt en gestript
- half dupont kabeltje op gele kabel van sensor gesoldeerd en stukje tape er om heen gedaan
- Linker sensor aangesloten op A0
- Rechter sensor aangesloten op A1
- Sensoren met kleine tie-wraps voor op mandje gemonteerd
Stap 7. Bumper gemaakt en aangesloten
- Voor de bumper heb ik tweede smalle strookjes kunststof uit een restantje kabelgoot deksel gezaagd. Ongeveer 2,5cm bij 25cm. Op de ene strook bevestig met wat lijm twee dopjes. In de andere strook boor ik acht kleine gaatjes voor de pootjes van de schakelaars. De pootjes van de schakelaars druk ik door de gaatjes en buig ik aan de achterkant om. Voor de stevigheid ook wat lijm tussen de schakelaar en het kunststof strookje aangebracht. Vervolgens met enig gewicht erop de lijm laten uitharden.
- Daarna stukjes 2-aderig draad aan de schakelaars gesoldeerd, twee gaatjes door de bumper en het mandje geboord en de bumper met twee kleine boutjes en moertjes aan het mandje bevestigd
- De min draad van de bumpers aangesloten op de – (min) terminal van het voedingsblokje, en op de plus draden een half jumperkabeltje gesoldeerd en de soldering omwikkeld met tape.
- Vervolgens de bumperschakelaar als volgt op de Arduino aangesloten:
Bumperschakelaar (vooraanzicht) | Arduino |
Links | D7 |
Rechts | D8 |
Stap 7. 12v Voeding aangesloten
Tot nu toe getest met de 5v voeding van de Arduino. In deze stap bereid ik het systeem voor op de definitieve voeding vanuit een 3-cellige Lipo batterij die ~ 12v levert.
De volgende componenten worden van een 12v voedingsstroom voorzien vanuit de Lipo accu:
- Motoren
De H-bridge zorgt voor een galvanische scheiding tussen de 5v aansturing vanuit de Arduino, en de 12v voedingsstroom vanuit de Accu. Hiervoor zijn de +12 v VCC en GND aangesloten op de VIN en GND aansluitingen van de MOS Fet driver.
- Fan
De Fan wordt aangestuurd door de MOS Fet driver. Deze zorgt voor een galvanische scheiding tussen de 5v aansturing vanuit de Arduino, en de 12v voedingsstroom vanuit de Accu. De MOS Fet driver is rechtstreeks aangesloten op de accu (VIN en GND) en fungeert als distributiepunt voor de 12v aansluitingen.
- Voltage devider voor het aanleveren van de batterijspanning
In de volgende stap beschrijf ik de realisatie van de voltage devider voor het aanleveren van de batterijspanning. Deze voltage devider wordt ook gevoegd vanuit de VIN en GND aansluitingen van de MOS Fet driver
- Arduino
Tot slot wordt ook de Arduino gevoed vanuit de 12v Accu. De Arduino Uno die ik gebruik kan een ingangsspanning tussen 7 en 12v verdragen op de power jack. Dus de Arduino kan rechtstreeks vanuit de 12v accu gevoed worden.
De volgende componenten worden van een 5v voedingsstroom vanuit de Arduino gevoed:
- Sensoren
De Sharp sensoren worden middels het voedingsblokje wat ik in stap 6 heb gemaakt gevoed vanuit de 5v aansluiting van de Arduino.
- Schakelaars bumper
De schakelaars voor de bumper hebben geen voeding nodig, wel de min aangesloten op het voedingsblokje uit stap 6.
- H-bridge
De H-bridge wordt middels het voedingsblokje wat ik in stap 6 heb gemaakt gevoed vanuit de 5v aansluiting van de Arduino.
- Ledje
Wordt gevoed vanuit de digitale poort van de Arduino waar hij op is aangesloten.
- Mos fet driver
De MOS Fet driver heeft geen voeding nodig, wel is de min aangesloten op een GND aansluiting van de Arduino om een floating ground te voorkomen.
- Wemos D1 mini (tijdelijk)
De Wemos D1 mini is met een jumperkabeltje van de 5V pin op de + van het voedingsblokje aangesloten, de GND pin op de – van het voedingsblokje. (De 3.3v aansluiting van de Arduino levert te weinig amperage om de Wemos D1 mini op te laten starten, vandaar op de 5V aangesloten)
Stap 8. Voltage devider maken om de batterij spanning door te geven aan de Arduino.
De Lipo accu kan defect raken als de spanning onder de 3v raakt. Daarom zit in de software code een functie die de robotstofzuiger uitzet als de batterij spanning te laag wordt. Uiteraard is het niet verstandig om de 12v accu rechtstreeks op een analoge poort van de Arduino aan te sluiten, dat zou het einde van de Arduino betekenen. Daarom wordt middels een weerstand (R2) en een potentiometer (R1) een Voltage devider gerealiseerd.
- De 10K potentiometer heeft drie pinnen, van links naar rechts: VCC, Signal, GND. Deze potentiometer heb ik op de vrije ruimte van de printplaat van het voedingsblokje uit stap 6 gesoldeerd. Op deze printplaat bevestig ik in de volgende stap ook de MOS Fet driver t.b.v. de fan.
- Tussen de Gnd aansluiting van de MOS Fet driver en de Gnd pin van de potentiometer heb ik een 1k Ohm weerstand gesoldeerd.
- Tussen de VIN aansluiting van de MOS Fet driver en de vcc pin heb ik een stukje draad gesoldeerd.
- Op de Signal pin van de potentiometer heb ik een male jumper kabeltje gesoldeerd waarvan ik aan één kant het stekkertje heb afgeknipt.
- Daarna heb ik met behulp van een voltmeter de potentiometer dusdanig afgesteld dat deze precies 5v levert.
- Daarna de jumperkabel van de Voltage devider aangesloten op analoge poort 4 (A4) van de Arduino.
Stap 9. MOS Fet driver aangesloten.
- Het MOS Fet bordje mbv van 2 boutjes en moertjes op de printplaat van het voedingsblokje uit stap 6 bevestigd. Hiervoor met een 2.5mm boortje 2 gaatjes in de printplaat geboord.
- De pinnen van de aansluitterminals van het MOS Fet bordje steken daarbij door de gaatjes van de printplaat.
- Met een flinke dot tin deze terminal pinnen vastgesoldeerd aan de printplaat en daarmee aansluitmogelijkheden gecreëerd voor o.a. de Voltage devider.
- Op de VIN en GND terminal alle 12v kabeltjes aangesloten
- Op de V+ en V- terminal de fan aangesloten
- Tussen de Sig(nal) aansluiting en de D12 poort van de Arduino een female-male jumperkabel bevestigd
- Tot slot op de Gnd aansluiting een female-male jumperkabel aangesloten en de jumperkabel aangesloten op een GND aansluiting van de Arduino aangesloten
Stap 10. De behuizing printen
Trek hier maar een paar dagen voor uit. Het printen van de verschillende componenten van de behuizing is een tijdrovende bezigheid. De behuizing bestaat uit de volgende onderdelen met daarachter de benodigde printtijd en PLA in grammen.
Onderdeel | Aantal | Kleur | Doorlooptijd | Doorlooptijd totaal | Gram | Totaal gram | File/Download |
Onderkant Container | 1 | Grijs | 426 | 426 | 56 | 56 | Con_3_bottom_58mm.stl |
Bovenkant Container | 1 | Grijs | 91 | 91 | 14 | 14 | Con_3_Top.stl |
Filter | 1 | Grijs | 25 | 25 | 3 | 3 | FilterTap.stl |
Filtercover | 1 | Grijs | 41 | 41 | 5 | 5 | FilterCover1.1.stl |
Cover Robot | 1 | Grijs | 711 | 711 | 111 | 111 | Vacuum_Robot_top_9.stl |
Bodemplaat Robot | 1 | Grijs | 1721 | 1721 | 230 | 230 | Vaccum_Robot_bottom_9.0.1_bottom.stl |
Bumper | 1 | Oranje | 51 | 51 | 8 | 8 | Bumper2.stl |
Button | 2 | Wit | 3 | 6 | 0 | 0 | button1#mmbasewidth.stl |
Button support 12mm (2x) | 1 | Wit | 22 | 22 | 2 | 2 | buttonSupport_12mm.stl |
Sensor support | 4 | Wit | 11 | 44 | 1 | 4 | Sharp_Support.stl |
PCB support | 2 | Wit | 16 | 32 | 2 | 4 | PCBSupport.stl |
SUM van Doorlooptijd | Kleur | SUM van Gram | |||||
3118 | Grijs | 419 | |||||
Oranje | 8 | ||||||
Wit | 5 | ||||||
Eindtotaal | 432 |
Ik heb op mijn Creality Ender 5 Pro geprint met de volgende instellingen:
Hierbij de volgende printersettings gehanteerd
Setting | Waarde |
Extruder temperature | 200o |
Hotbed temperature | 50o |
Layer height | 0,2 mm |
Infill density | 30% cubic |
Wall/Top thickness | 1,2 mm |
Generate support | Uitgevinkt |
Retraction | Aangevinkt |
Printing Speed | 80mm/s |
Cooling | 100% |
En hier de verschillende geprinte onderdelen
Stap 11. Montage
Het mandje heb ik ontmanteld en monteer nu stap voor stap de verschillende componenten in de geprinte behuizing. Te beginnen met de aandrijfwielen. Deze bevestig ik met twee M2.5 boutjes en moertjes. Deze moertjes vallen precies in de uitsparing van de bracket van de aandrijfwielen.
Daarna is de caster ball aan de beurt. De caster ball vastgezet met 2 M5 12mm boutjes en moeren. De gaatjes van de caster ball kwamen niet overeen met de gaatjes in de bodem van de behuizing, dus met een 5mm metaalboortje twee extra gaatjes geboord.
De moment buttons voor de bumper die ik heb gebruikt wijken af van degene die Cesar heeft toegepast, dus de button-support van Cesar kon ik niet toepassen. M.b.v. FreeCAD heb ik twee vergelijkbare support gemaakt, deze zijn geschikt voor buttons van 12x12mm.
Noot: De originele button-support kun je downloaden van ThingiVerse: https://www.thingiverse.com/thing:2288841
Op de onderkant van de button een drup Lock-tide lijm gedaan. De pootjes van de button passen (na enig rechtbuigen) om het middenstuk. Druk de button geheel naar beneden. Op het bevestigingspuntje op de bodem van de behuizing ook een drup Lock-tide lijm gedaan. Voor het plaatsen van de support inclusief button moest ik het voorpaneel van de bodem iets naar buiten buigen om het knopje door het gaatje te kunnen krijgen. Druk de button support op het bevestigingspuntje op de bodem van de behuizing. Plaats tijdelijk een helft van een wasknijper tussen de support en de inlaat van de bodemplaat om de support te fixeren terwijl de lijm droogt.
De Sharp sensors heb ik met twee M2.5 10mm boutjes met moertjes vastgezet op de Sharp support, vervolgens met een derde M2.5 boutje en moertje vastgezet op de bodem van de behuizing en de stekker (met driekleurige draden) in de daarvoor bestemde terminal op de sensor geduwd.
De H-bridge heb ik met 4 2.5mm nylon spacers en 8 2.5mm nylon boutjes op de bodem van de behuizing geschroefd
Voor het bevestigen van de printplaat met de Mosfet Driver module, Voltage devider voor de batterijspanning en aansluitterminals voor 5v en GND heb ik met behulp van FreeCAD twee bevestigingssteunen gemaakt en geprint. Deze bevestigingssteunen met 4 M2 10mm boutjes en moertjes vastgezet op de PCB. Vervolgens met een 3mm metaalboortje twee gaatjes in de bodem van de behuizing geplaatst (van vooraf gezien, rechtsvoor). De PCB een beetje schuin geplaatst, anders paste het niet met de rechter Sharp sensor. Vervolgens alle bedrading (voeding Sharp Sensors, GND bumper buttons, voeding H-Bridge, 12V voeding (VIN, GND) Mosfet Driver module. 5v+GND Arduino) aangesloten op de PCB en de PCB met behulp van 2 M2.5 10mm boutjes en moertjes vastgezet op de bodem.
De Arduino Uno wordt met een M2.5 10mm bout + moer aan een oogje op de bodem vastgeschroefd. Maar eerst alle jumperkabeltjes aangesloten conform de pinout eerder in dit document.
In het deksel van de behuizing zijn 2 gaatjes aangebracht t.b.v. LED’s. Uit de beschrijving van Cesar haal ik maar één LED, dus waar het tweede gaatje voor bedoeld is is mij onduidelijk. M.b.v. een druk Lock-tide lijm in één van de gaatjes een rode LED gelijmd. Aan de binnenkant van het deksel aangegeven wat de + en – is van de LED en vervolgens de pootjes op “headerlengte” afgeknipt zodat hier twee jumperkabeltjes opgeschoven kunnen worden. M.b.v. een stukje krimpkous een soort stekkertje gemaakt van twee female-female jumperkabels en deze op de LED geschoven. Op de andere zijde ook een stukje krimpkous geplaatst. Idem dito met één zijde van twee male-male jumperkabeltjes in dezelfde kleuren. De andere zijde aangesloten op pin 13 en GND van de Arduino UNO. Op deze wijze kun je eenvoudig het deksel losmaken van de bodem bij eventueel onderhoud. De met krimpkous omklede female en male jumperkabels fungeren hierbij als stekker en contra-stekker.
De bumper geassembleerd door de twee “button 1mm base” items met een drup Lock-tide lijm op de bumper gelijmd, en vervolgens de bumper op de moment buttons gelijmd.
Nu is het tijd voor een proefritje.
Stap 12. App ontwikkeld
Omdat het wachten was op wat onderdelen en een 3D printer, gestart met het ontwikkelen van een app.
Use cases:
- Indicatie batterijniveau V
- Notificatie lege batterij V
- Indicatie dat batterij opgeladen wordt
- Notificatie stofbak vol (nog niet mogelijk, sensor ontbreekt)
- Notificatie robotstofzuiger heeft zichzelf vast gereden
- Debugging mogelijkheden, uitlezen waarden diverse sensoren V
- Instellen dagen/tijden dat robotstofzuiger operationeel mag zijn
De opzet is om alles wat op de seriële poort (RX, TX) van de Wemos D1 mini binnenkomt als een UDP broadcast bericht te verzenden naar poort 6666. Hiervoor dient de Wemos D1 mini te fungeren als een UDP Broadcast server. Hiervoor een Arduino sketch ontwikkeld.
Op de Arduino Uno draait een aparte (semi)thread aangestuurd vanuit van de Loop() procedure die er voor zorgt dat er een JSON bericht met de verschillende waarden van de sensoren aangeboden wordt op de seriële poort van de Wemos D1 mini. De app is geabonneerd op de UDP server en vertaald de sensorinformatie naar gebruikersfunctionaliteit.
Overzicht gebruikte software per component:
Component | Software en versie | Download locatie |
Wemos D1 mini | Arduino sketch | |
Arduino Uno Rev3 WiFi | Aangepaste versie van VacuumCode 3.0.1 | volgt |
App | Android Studio 2020.3.1 patch 3 | https://developer.android.com/studio/ |
ESP8266
De eerste stap is MicroPython installeren op de Wemos D1 mini, volg hiervoor stap 1 t/m 3 van mijn tutorial “MicroPython flashen op Wemos D1 mini”. De tutorial vindt je hier: https://www.sydspost.nl/index.php/2022/03/12/micropython-flashen-op-wemos-d1-mini/
Na tientallen vruchteloze pogingen om AT command firmware of m’n eigen ontwikkelde sketch te installeren op de ingebouwde ESP8266 van de Arduino UNO R3 Wifi heb ik dat opgegeven. Heb het vermoeden dat de chip defect is. Daarom uitgeweken naar een Wemos D1 mini in afwachting van een nieuwe Arduino Uno R3 WiFI.
De sketch maakt gebruik van de NTP client bibliotheek voor het synchroniseren van de tijd met time.google.com, daarom eerst in Arduino IDE onder Hulpmiddelen -> Bibliotheken beheren de bibliotheek “NTPClient_Generic” versie 3.5.2 toegevoegd.
Daarnaast wordt TimedAction (soort van quasi multi threading) gebruikt om eens in de minuut de tijd te synchroniseren. Deze bibliotheek kun je downloaden via https://playground.arduino.cc/uploads/Code/TimedAction-1_6/index.zip, en vervolgens in Arduino IDE inlezen via Schets -> Bibliotheek gebruiken -> Voeg .zip bibliotheek toe
Daarna m.b.v. Arduino IDE Schets -> Upload de volgende sketch op de Wemos D1 mini geladen, dit met de volgende settings (Onder Hulpmiddelen):
#include <ESP8266WiFi.h> #include <WiFiUdp.h> // Set WiFi credentials #define WIFI_SSID "Your SSID here" #define WIFI_PASS "Your WiFi password here" #define UDP_PORT 6666 // UDP WiFiUDP UDP; IPAddress broadcastIp(192, 168, 2, 255); // Change your subnet here void setup() { // Setup serial port Serial.begin(9600); Serial.println(); // Begin WiFi WiFi.begin(WIFI_SSID, WIFI_PASS); // Connecting to WiFi... Serial.print("Connecting to "); Serial.print(WIFI_SSID); // Loop continuously while WiFi is not connected while (WiFi.status() != WL_CONNECTED) { delay(100); Serial.print("."); } // Connected to WiFi Serial.println(); Serial.print("Connected! IP address: "); Serial.println(WiFi.localIP()); } void loop() { if (Serial.available() > 0) { UDP.beginPacket(broadcastIp, UDP_PORT); UDP.println(Serial.readString()); UDP.endPacket(); } }
Tot slot de Wemos D1 mini opnieuw opgestart en getest of een en ander werkt. Hiervoor een klein python programmaatje ontwikkeld die op een linux of windows machine kan draaien:
pi@raspberrypi:~ $ cat receiveudp.py from socket import * s=socket(AF_INET, SOCK_DGRAM) s.bind(('192.168.2.255',6666)) while True: m=s.recvfrom(1024) print m[0] pi@raspberrypi:~ $ python receiveudp.py
Open binnen Arduino IDE onder Hulpmiddelen de seriële monitor, stel de baud rate in op 9600 baud. Type naast de button Verzenden bijv. de tekst “Hello” in, en klik op verzenden.
Als het goed is komt dit bericht nu aan in je python programmaatje
pi@raspberrypi:~ $ python receiveudp.py Hello
Arduino Uno Rev3 WiFi
De code van Cesar Nieto, Vacuumcode 3.0.1 heb ik als basis gebruikt, en deze uitgebreid met de functionaliteit om JSON-berichten met sensorwaarden via een Software seriele verbinding (D10 RX, D11 TX) te verzenden naar de Wemos D1 mini. Deze aanpaste sketch m.b.v. Arduino IDE en onderstaande settings op de Arduino Uno gezet. Crusiaal is om op het juiste moment wanneer Arduino IDE klaar is met compileren en de sketch gaat uploaden even op de rode reset button te drukken op de Arduino Uno.
//Code: VacuumCode_Encoders //Version: 4.0.1 //Author: Cesar Nieto refer to ces.nietor@gmail.com //UDP-server added by: Syds Post, sydspost@gmail.com //Last change: 16/11/2021 //Last Changes: Code added to support Pololu Motor Encoders, pins have been changed. // A PD controller is used to control the speed of the motors. // UDP-server added #include <SoftwareSerial.h> #include <ArduinoJson.h> #include <math.h> #include <TimedAction.h> ////////////PINS//////////////// // Distance Analog Sensors (Sharp) #define SD1 (0) //left front sensor #define SD2 (1) //right front sensor #define SD3 (2) //left side sensor #define SD4 (3) //right side sensor // RX/TX pins ESP8266 #define RX (10) // RX #define TX (11) // TX // Battery Voltage input #define battery (4) //Analog // IndicatorLED #define led (13) // Fan output #define fanmotor (12) // the number of the LED pin // Motor1 Right #define motor1Pin1 (2) #define motor1Pin2 (4) // #define encodPinA1 (2) // encoder A pin, interrupt pin of Arduino Uno // #define encodPinB1 (4) // encoder B pin, read motor direction // Motor2 Left #define motor2Pin1 (5) #define motor2Pin2 (6) // #define encodPinA2 (3) // encoder A pin, interrupt pin of Arduino Uno // #define encodPinB2 (7) // encoder B pin, read motor direction // Bumper #define bumper1 (7) #define bumper2 (8) // PWM for the micro metal motors //Values to delete soon since use of PID #define pwmMax (160) #define pwmMin (0) // PID loop time #define LOOPTIME (100) ///////////////Constants//////////////// const float voltageBatCharged = 12.0; // Voltage measured when battery fully charged //Change this const float batteryLimitDischarge = 11.6; // Safe value to not kill the Battery const String WIFI_SSID = "H369ABF8AF9"; // Your WiFi ssid //Change this const String PASSWORD = "5A5C4A653C59"; // Password //Change this const String HOSTNAME = "RoboVac"; // Change this const String strPortnumber = "6666"; // Change this const String strSubnet = "192.168.2.255"; // Change this // Variables will change: int bumperState1 = 0; // variable for reading the pushbutton status int bumperState2 = 0; // variable for reading the pushbutton status int counter = 0; // Prevents from being stuck boolean control = true; boolean printOnce = true; //For debugging unsigned long lastMilli = 0; // loop timing unsigned long lastMilliPrint = 0; unsigned long lastErrorMilli = 0; //Motor 1 float speed_req1 = 60.0; // speed (Set Point) float speed_act1 = 0.0; // speed (actual value) int PWM_val1 = 0; // (25% = 64; 50% = 127; 75% = 191; 100% = 255) volatile long count_1 = 0; // rev counter //Motor 2 float speed_req2 = 60.0; float speed_act2 = 0.0; int PWM_val2 = 0; volatile long count_2 = 0; float Kp = 0.6;// 0.65; // PID Proportional control Gain (Good values as well: 0.5) float Kd = 0.0;// 0.005; // PID Derivitave control Gain // Setup SoftwareSerial port for ESP8266 SoftwareSerial esp8266(RX, TX); // Initialize JSON object StaticJsonDocument<255> jsonDocument; JsonObject root = jsonDocument.to<JsonObject>(); //////////////CODE///////////// void setup() { // Initialize serial Serial.begin(9600); // Initialize SoftwareSerial ESP8266 esp8266.begin(9600); //Initialize outputs and inputs //Fan motor as output pinMode(fanmotor, OUTPUT); //Motor1 pinMode(motor1Pin1, OUTPUT); pinMode(motor1Pin2, OUTPUT); //Motor2 pinMode(motor2Pin1, OUTPUT); pinMode(motor2Pin2, OUTPUT); //LED pinMode(led, OUTPUT); //INPUTS //Initialize the pushbutton inputs //Motor encoders //pinMode(encodPinA1, INPUT_PULLUP); //pinMode(encodPinB1, INPUT_PULLUP); //pinMode(encodPinA2, INPUT_PULLUP); //pinMode(encodPinB2, INPUT_PULLUP); //Bumper pinMode(bumper1, INPUT_PULLUP); pinMode(bumper2, INPUT_PULLUP); //Sensor pinMode(SD1, INPUT); pinMode(SD2, INPUT); pinMode(SD3, INPUT); pinMode(SD4, INPUT); //Batt pinMode(battery, INPUT); //Encoder Interrupt executes rencoder_# function when falling edge of the signal //Refer to: https://www.arduino.cc/en/Reference/AttachInterrupt //Arduino UNO only has 2 external interrupts attachInterrupt(0, rencoder_1, FALLING); //Pin 2 of Arduino Uno attachInterrupt(1, rencoder_2, FALLING); //Pin 3 of Arduino Uno ///////////////////////////////Wait//////////////////////////////////////// //Wait about 5 s and initialize fan if voltage ok Serial.println("Starting..."); waitBlinking(3,1); //5 seconds at 1 Hz //Crank (initialize the fan because the voltage drops when cranking) if(readBattery(battery)>=voltageBatCharged){ digitalWrite(fanmotor, HIGH); //Turn the Fan ON Serial.println("Fan: on"); delay(500); //For 1000ms } else { //do nothing Convention } } void sendJSON(){ //Build up JSON message root["Payload"]["BatteryLevel"] = readBattery(battery); // BatteryLevel root["Payload"]["Left front sensor"] = sdSHARP(SD1); // Left front sensor root["Payload"]["Right front sensor"] = sdSHARP(SD2); // Right front sensor root["Payload"]["Left side sensor"] = sdSHARP(SD3); // Left side sensor root["Payload"]["Right side sensor"] = sdSHARP(SD4); // Right side sensor root["Payload"]["Left bumper"] = digitalRead(bumper1); // Left bumper root["Payload"]["Right bumper"] = digitalRead(bumper2); // Right bumper root["Payload"]["Fan"] = digitalRead(fanmotor); // Fan motor String data; serializeJson(root, data); // Send JSON payload sendData(data); } // Initialize protothread TimedAction sendThread = TimedAction(1000,sendJSON); /////////////////////////////////////////////////MAIN CODE////////////////////////////// void loop(){ //Keep the control of the battery automatically turn the fan off //If control = true the battery level is ok, otherwise the battery is . batteryControl(battery); //modifies the variable control of the battery is low setMotors(); //Set pwm of each motor according to the actual speed controlRobot(); // Execute all conditions to move printMotorsInfo(); sendThread.check(); // Check thread } //////////Functions To Use ////////// //PORTD, containts Digital pins 0-7, 4 and 7 used to consider the direction of each motor. (PIND is for read only) void rencoder_1() { // pulse and direction, direct port reading to save cycles if (PIND & 0b00010000) count_1++; // if(digitalRead(encodPinB1)==HIGH) count ++; else count_1--; // if (digitalRead(encodPinB1)==LOW) count --; } void rencoder_2() { // pulse and direction, direct port reading to save cycles if (PIND & 0b10000000) count_2++; // if(digitalRead(encodPinB2)==HIGH) count ++; else count_2--; // if (digitalRead(encodPinB2)==LOW) count --; } void waitBlinking(int n, int frequency){ //blink for n seconds at frequency hz for (int i=1; i <= n; i++){ for(int j=1; j<=frequency; j++){ digitalWrite(led, HIGH); delay((1000/frequency)/2); //Half time on digitalWrite(led, LOW); delay((1000/frequency)/2); //Half time off } } } double sdSHARP(int Sensor){ //Returns the distance in cm double dist = pow(analogRead(Sensor), -0.857); // x to power of y return (dist * 1167.9); } float readBattery(int input){ int readInput; float voltage; readInput = analogRead(input); voltage = (((readInput*4.9)/1000)*voltageBatCharged ) / 5; // resolution of analog input = 4.9mV per Bit resolution Serial.print(" Battery= "); Serial.println(voltage); return voltage; } void batteryControl(int input){ //Turn everything off in case the battery is low float v_battery; v_battery = readBattery(input); if(v_battery<=batteryLimitDischarge){ //battery limit of discharge, Don't put this limit lower than 11.1V or you can kill the battery control = false; } else { //Do nothing Convention } } void moveMotors(int moveTime, int pwmMotor1, int pwmMotor2, char direc){ //Manipulate direction according the desired movement of the motors switch(direc){ case 'f': analogWrite(motor1Pin1, pwmMotor1); analogWrite(motor1Pin2, 0); //PWM value where 0 = 0% and 255 = 100% analogWrite(motor2Pin1, pwmMotor2); analogWrite(motor2Pin2, 0); delay(moveTime); break; case 'b': analogWrite(motor1Pin1, 0); analogWrite(motor1Pin2, pwmMotor1); analogWrite(motor2Pin1, 0); analogWrite(motor2Pin2, pwmMotor2); delay(moveTime); break; case 'r': analogWrite(motor1Pin1, 0); analogWrite(motor1Pin2, pwmMotor1); analogWrite(motor2Pin1, pwmMotor2); analogWrite(motor2Pin2, 0); delay(moveTime); break; case 'l': analogWrite(motor1Pin1, pwmMotor1); analogWrite(motor1Pin2, 0); analogWrite(motor2Pin1, 0); analogWrite(motor2Pin2, pwmMotor2); delay(moveTime); break; case 's': analogWrite(motor1Pin1, 0); analogWrite(motor1Pin2, 0); analogWrite(motor2Pin1, 0); analogWrite(motor2Pin2, 0); delay(moveTime); break; default: //Do nothing convention Serial.println("Default"); break; } } void getMotorsSpeed() { static long countAnt_1 = 0, countAnt_2 = 0, countAnt_3 = 0, countAnt_4 = 0; // last count, static variables preserve the last value speed_act1 = ((count_1 - countAnt_1)*(60*(1000/LOOPTIME)))/(3*298); // 3 pulses X 298 gear ratio = 894 counts per output shaft rev countAnt_1 = count_1; speed_act2 = ((count_2 - countAnt_2)*(60*(1000/LOOPTIME)))/(3*298); countAnt_2 = count_2; } int updatePid(int command, int targetValue, int currentValue) { // compute PWM value float pidTerm = 0; // PID correction float error = 0; static float last_error = 0; unsigned long reachTime = 0; error = abs(targetValue) - abs(currentValue); Serial.print(" Error: "); Serial.print(error); /*if (error <= 1.0 && printOnce){ //Measure time to see when the motor reached the Set point reachTime = millis()-lastErrorMilli; Serial.print("SP reachead: "); Serial.print(reachTime); Serial.println(); printOnce = false; } */ //PID controller, not using Ki at the moment pidTerm = (Kp * error) + (Kd * (error - last_error)); last_error = error; return constrain(command + int(pidTerm), 0, 255); } void setMotors(){ if((millis()-lastMilli) >= LOOPTIME) { // enter tmed loop lastMilli = millis(); getMotorsSpeed(); // calculate speed, volts and Amps //Global values PWM_val1 = updatePid(PWM_val1, speed_req1, speed_act1); // compute PWM value PWM_val2 = updatePid(PWM_val2, speed_req2, speed_act2); // compute PWM value } } void printMotorsInfo() { // display data if((millis()-lastMilliPrint) >= 500) { lastMilliPrint = millis(); Serial.print(" SP_1:"); Serial.print(speed_req1); Serial.print(" RPM_1: "); Serial.print(speed_act1); Serial.print(" PWM_1: "); Serial.print(PWM_val1); Serial.print(", SP_2: "); Serial.print(speed_req2); Serial.print(" RPM_2: "); Serial.print(speed_act2); Serial.print(" PWM_2: "); Serial.print(PWM_val2); Serial.println(); } } void sendData(String data) { esp8266.println(data); Serial.println(data); } void controlRobot(){ Serial.print(" SD1= "); Serial.print(sdSHARP(SD1)); Serial.println(); Serial.print(" SD2= "); Serial.print(sdSHARP(SD2)); Serial.println(); //delay(200);*/ float minDistanceSharp = 5; // Distance in cm bumperState1 = digitalRead(bumper1); bumperState2 = digitalRead(bumper2); Serial.print("Bumper 1: "); Serial.println(bumperState1); Serial.print("Bumper 2: "); Serial.println(bumperState2); if (control){ digitalWrite(led, HIGH); if (sdSHARP(SD1)<= minDistanceSharp){ //If the distance between an object and the left front sensor is less than 4.3 cm or the bumper hits, it will move to the left if (counter == 2){ // prevent of being stuck on corners counter = 0; } else { //Do nothing Convention } moveMotors(100, PWM_val1, PWM_val2, 'f'); // approach a bit moveMotors(500, PWM_val1, PWM_val2, 'b'); // backward delay of 500ms moveMotors(300, PWM_val1, PWM_val2, 'l'); counter = counter + 2; Serial.println(" Turn Left "); } else if (sdSHARP(SD2)<= minDistanceSharp){ //If the distance between an object and the right front sensor is less than 4.3 cm, it will move to the right if (counter == 1){ counter = 0; } else{ //Do nothing Convention } moveMotors(100, PWM_val1, PWM_val2, 'f'); moveMotors(500, PWM_val1, PWM_val2, 'b'); moveMotors(300, PWM_val1, PWM_val2, 'r'); counter++; Serial.println(" Turn Right"); } else if (bumperState1==0){ counter = 0; moveMotors(500, PWM_val1, PWM_val2, 'b'); moveMotors(300, PWM_val1, PWM_val2, 'l'); Serial.println(" Turn Left "); } else if (bumperState2==0){ counter = 0; moveMotors(500, PWM_val1, PWM_val2, 'b'); moveMotors(300, PWM_val1, PWM_val2, 'r'); Serial.println(" Turn Right "); } else { if(counter==3){ //Corner moveMotors(1000, PWM_val1, PWM_val2, 'l'); counter = 0; } else { moveMotors(300, PWM_val1, PWM_val2, 'f'); Serial.println(" Move Forward"); } } } else if (!control){ //If the battery is low, turn everything off digitalWrite(fanmotor, LOW); //Turn the Fan OFF Serial.println("Fan: Off"); moveMotors(0, 0, 0, 's'); Serial.println("Stop motors"); Serial.print(" Low Battery! "); Serial.println(); waitBlinking(1,3); //blink as warning 3hz in a loop } Serial.println(); }
Voor test bovenstaande sketch geflashed naar de Arduino conform beschrijving in stap 1 en een m.b.v. een breadboard een testopstelling gemaakt. Hierbij de Wemos D1 mini als volgt op de Arduino Uno Rev3 Wifi aangesloten:
Arduino Uno | Wemos D1 mini | Toelichting |
3v3 | 3v3 | |
Gnd | Gnd | |
10 | Tx | |
11 | Rx | Tijdelijk rechtstreeks aangesloten, komt nog Level converter tussen om het output voltage van de Arduino Uno (D11 TX) terug te brengen naar 3.3v |
Vervolgens opnieuw getest. In Arduino IDE onder Hulpmiddelen de Seriële monitor gestart en de baudrate op 9600 ingesteld, deze geeft de volgende output:
Vervolgens op m’n raspberry pi het volgende python scriptje gestart om te testen of de UDP broadcast werkt:
pi@raspberrypi:~ $ cat receiveudp.py from socket import * s=socket(AF_INET, SOCK_DGRAM) s.bind(('192.168.2.255',6666)) while True: m=s.recvfrom(1024) print m[0] pi@raspberrypi:~ $ python receiveudp.py
Na het scriptje met python te hebben gestart geeft dit de volgende output:
pi@raspberrypi:~ $ python receiveudp.py {"Payload":{"BatteryLevel":3.7044,"Left front sensor":10.08194,"Right front sensor":10.46865,"Left side sensor":9.916189,"Right side sensor":9.365329,"Left bumper":1,"Right bumper":1,"Fan":0}} {"Payload":{"BatteryLevel":2.94,"Left front sensor":8.851206,"Right front sensor":8.628358,"Left side sensor":8.701281,"Right side sensor":9.006843,"Left bumper":1,"Right bumper":1,"Fan":0}} {"Payload":{"BatteryLevel":2.68128,"Left front sensor":11.70863,"Right front sensor":11.61608,"Left side sensor":12.19643,"Right side sensor":12.9008,"Left bumper":1,"Right bumper":1,"Fan":0}}
Eureka ! Het werkt. Nu dit werkt heb ik bovenstaande code verwerkt in een nieuwe versie van de VacuumCode sketch van Cesar Nieto. Zodra deze volwassen genoeg is zal ik dit in deze blog publiceren en tevens als een verbetering op de versie 3.0.1 van Cesar publiceren op Github.
App
De app heb ik m.b.v. Android Studio gebouwd in Kotlin. Meer info volgt zodra de app volwassen genoeg is om te delen. Ook deze zal ik te zijne tijd hier en op Github publiceren.
doc
{"Command": "SetOpTimes", "Weekdays": [["09:00", "17:00"], ["09:00", "17:00"], ["09:00", "17:00"], ["09:00", "17:00"], ["09:00", "17:00"], ["09:00", "17:00"], ["09:00", "17:00"]]} {"Command": "GetOpTimes"} {"Command": "ResultGetOpTimes","Weekdays":[["00:00","23:59"]["00:00","23:59"]["00:00","23:59"]["00:00","23:59"]["00:00","23:59"]["00:00","23:59"]["00:00","23:59"]]} {"Command":"SetTime","DateTime":"(2022, 3, 21, 17, 14, 42, 0, 80)"}