Robotstofzuiger

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.

Links

https://www.instructables.com/Build-Your-Own-Vacuum-Robot/

Materiaallijst

ArtikelAantalKosten per stukTotale kostenTe bestellen bij:
Arduino Uno Wifi15,895,89Aliexpress
IRF520 MOS FET Driver Module11,451,45Aliexpress
H-bridge L298 Dual Motor Driver12,052,05Aliexpress
Gearmotor DC 12V 101RPM + Wheel Kit24,6112,05Aliexpress
Fan blower AVC BA10033B12G DC 12V 4.5A114,3014,30Aliexpress
Sharp Distance Sensor GP2Y0A41SK0F23,099,52Aliexpress
ZIPPY Compact 1300mAh 3S 25C Lipo Pack110,2910,29HobbyKing
XT-60H-M male stekker10,670,67TME
2k Ohm potentiometer12,282,28Aliexpress
M3 schroeven + boutjes204,13Aliexpress
Ball caster metal ball11,771,77Aliexpress
Pushbuttons 25 stuks21,561,56Aliexpress
#8-32 x 2 inch bouten + moeren + ring20,110,22Plat
Vacuum bag filter (cloth type)1uit WTW filter
On/Off button switch red 12v momentary 11,041,04Aliexpress
Socket voor button10,620,62Aliexpress
Rood/zwarte draad 5m12,572,57Aliexpress
LiPo Battery Charger 3s16,836,83Aliexpress
Loctite Glue-3 5g15,775,77Praxis
Schroef connectors 2 pin 10-stuks10,650,65Aliexpress
Jumper cables 30cm male-female11,821,82Aliexpress
5.5 Mm X 2.1 Mm 5.5X2.1 Dc Voeding Plug10,470,47Aliexpress
Stepdown buck converter10,910,91Aliexpress
PLA filament 1,75mm 1 kg119,5019,50123Inkt.nl
Totaal105,45

Pinout

Arduino1e component2e component
H-BridgeMOS Fet driver
VCC +12vVIN
GNDGND
D12SIG
GNDGND
5V5V
D2IN1
D4IN2
D5IN3
D6IN4
Sharp Sensor links
A0gele draad
5vrode draad
GNDZwarte draad
Sharp Sensor rechts
A1gele draad
5vrode draad
GNDzwarte draad
LED
D13lange poot (Anode)
GNDkorte poot (Kathode)
Bumper links
D7Rode draad
GNDZwarte draad
Bumper rechts
D8Rode draad
GNDZwarte draad
Batterij indicatie
A4Witte draad (vanaf potmeter)
GNDGND
Wemos D1 mini (tijdelijk)
3,3v3,3v
GNDGND
D10RX
D11TX

Stap 1. Arduino Uno geflashed met VacuumCode

  • Arduino IDE geupdatet naar versie 1.8.15
  • Arduino Uno WiFi Dev Ed library 0.0.3 geinstalleerd
  • VacuumCode 3.0.1 gedownloaded van https://content.instructables.com/ORIG/FQL/0HS7/J2UPCU7L/FQL0HS7J2UPCU7L.ino
  • De sketch FQLOHS7J2UPCU7L.ino geopend in Arduino IDE
  • Hulpmiddelen, Board = “Arduino Uno Wifi”
  • Hulpmiddelen, Poort = “COM5”
  • Dipswitches op Arduino Uno board correct ingesteld, dit conform dit schema:

Switch status and mode selection:

 12345678
CH340 connect to ESP8266 (upload sketch)OFFOFFOFFOFFONONONNoUSE
CH340 connect to ESP8266 (connect)OFFOFFOFFOFFONONOFFNoUSE
CH340 connect to ATmega328 (upload sketch)OFFOFFONONOFFOFFOFFNoUSE
Mega328+ESP8266ONONOFFOFFOFFOFFOFFNoUSE
All modules work independentOFFOFFOFFOFFOFFOFFOFFNoUSE

Dipswitches 3 en 4 op On gezet

  • Schets, upload
  • Hulpmiddelen, Seriele monitor
  • Binnen Seriele monitor, Baudrate op 9600 baud zetten, Arduino geeft volgende output:

Battery= 3.26  Low Battery !

Stap 2. Zaag en boor mal gemaakt voor prototype

  • Heb nog geen 3D-printer, dus ik maak eerst een werkend prototype 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 meebestelde 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 Arduino

  • H-bridge als volgt aangesloten
H-BridgeArduinoMOS Fet driver
VCC +12vVIN
GNDGND
5V5V
IN1D2
IN2D4
IN3D5
IN4D6

Stap 5. Led aangesloten

  • Led aangesloten op D13 en GND van de Arduino

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
LinksD7
RechtsD8

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. App ontwikkeld

Omdat het wachten is 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:

ComponentSoftware en versieDownload locatie
Wemos D1 miniArduino sketch
Arduino Uno Rev3 WiFiAangepaste versie van VacuumCode 3.0.1 volgt
AppAndroid Studio 2020.3.1 patch 3https://developer.android.com/studio/

ESP8266

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 UnoWemos D1 miniToelichting
3v33v3
GndGnd
10Tx
11RxTijdelijk 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": "2021-12-03T19:35:01Z"}

Geef een antwoord

Het e-mailadres wordt niet gepubliceerd. Vereiste velden zijn gemarkeerd met *