Robotstofzuiger

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)"}