The robot has the usual suspects, with the ESP8266 for the SOC, an HC-SR04 sonar on the front, two continuous revolution servos attached to the wheel on a hobby robot chassis. The schematic is actually from this page: https://www.instructables.com/id/HackerBoxes-0013-Autosport/ that is where I got the kit.
This is my own code, that is running on the 8266. It looks the same as most of the other MQTT nodes in the house:
// // 2WD NodeMCU controlled over MQTT (node-red) // Sonar distance measured at regular intervals sent over MQTT as well. // #include <ESP8266WiFi.h> #include <PubSubClient.h> #include <PubSubClientTools.h> #include <Thread.h> // https://github.com/ivanseidel/ArduinoThread #include <ThreadController.h> // Update these with values suitable for your network. const char* ssid = "raspi-webgui"; const char* password = "********"; const char* MQTT_SERVER = "10.3.141.1"; #define echoPin 12 // Echo Pin #define trigPin 13 // Trigger Pin #define LEDPin 16 // Onboard LED #define RightMotorSpeed 5 #define RightMotorDir 0 #define LeftMotorSpeed 4 #define LeftMotorDir 2 WiFiClient espClient; PubSubClient client(MQTT_SERVER, 1883, espClient); PubSubClientTools mqtt(client); ThreadController threadControl = ThreadController(); Thread thread = Thread(); long lastMsg = 0; char msg[50]; int value = 0; String s = ""; int maximumRange = 200; // Maximum range needed int minimumRange = 0; // Minimum range needed long duration, distance; // Duration used to calculate distance void setup() { // put your setup code here, to run once: pinMode(BUILTIN_LED, OUTPUT); // Initialize the BUILTIN_LED pin as an output Serial.begin(115200); setup_wifi(); // Connect to MQTT Serial.print(s+"Connecting to MQTT: "+MQTT_SERVER+" ... "); if (client.connect("ESP8266Client")) { Serial.println("connected"); mqtt.subscribe("wheel/Left", topic1_subscriber); mqtt.subscribe("wheel/Right", topic2_subscriber); } else { Serial.println(s+"failed, rc="+client.state()); } pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT); pinMode(RightMotorSpeed, OUTPUT); pinMode(RightMotorDir, OUTPUT); pinMode(LeftMotorSpeed, OUTPUT); pinMode(LeftMotorDir, OUTPUT); } void setup_wifi() { delay(10); // We start by connecting to a WiFi network Serial.println(); Serial.print("Connecting to "); Serial.println(ssid); WiFi.begin(ssid, password); while (WiFi.status() != WL_CONNECTED) { delay(500); Serial.print("."); } Serial.println(""); Serial.println("WiFi connected"); Serial.println("IP address: "); Serial.println(WiFi.localIP()); } /////////////////////////////////////////////////////////////////////// //////////// Motor Controls ////////////////////////////////////////// void rightStop() { digitalWrite(RightMotorSpeed, LOW); } void leftStop() { digitalWrite(LeftMotorSpeed, LOW); } void rightForward() { digitalWrite(RightMotorDir, HIGH); digitalWrite(RightMotorSpeed, HIGH); } void leftForward() { digitalWrite(LeftMotorDir, HIGH); digitalWrite(LeftMotorSpeed, HIGH); } void rightReverse() { digitalWrite(RightMotorDir, LOW); digitalWrite(RightMotorSpeed, HIGH); } void leftReverse() { digitalWrite(LeftMotorDir, LOW); digitalWrite(LeftMotorSpeed, HIGH); } ////////////////////////////////////////////////////////////////////////// ////////// Sonar Sample ////////////////////////////////////////////////// long sample() { /* The following trigPin/echoPin cycle is used to determine the distance of the nearest object by bouncing soundwaves off of it. */ digitalWrite(trigPin, LOW); delayMicroseconds(2); digitalWrite(trigPin, HIGH); delayMicroseconds(10); digitalWrite(trigPin, LOW); duration = pulseIn(echoPin, HIGH); //Calculate the distance (in cm) based on the speed of sound. distance = duration/58.2; if (distance >= maximumRange || distance <= minimumRange) { /* Send a negative number to computer and Turn LED ON to indicate "out of range" */ Serial.println("-1"); //digitalWrite(BUILTIN_LED, LOW); // Turn the LED on (Note that LOW is the voltage level } else { /* Send the distance to the computer using Serial protocol, and turn LED OFF to indicate successful reading. */ Serial.println(distance); //digitalWrite(BUILTIN_LED, HIGH); // Turn the LED off by making the voltage HIGH } return distance; } ////////////////////////////////////////////////////////////////////////// ////////// MQTT Stuff /////////////////////////////////////////////////// void reconnect() { // Loop until we're reconnected while (!client.connected()) { Serial.print("Attempting MQTT connection..."); // Attempt to connect if (client.connect("ESP8266Client")) { Serial.println("connected"); // Once connected, publish an announcement... client.publish("outTopic", "car connected"); // ... and resubscribe //client.subscribe("inTopic"); } else { Serial.print("failed, rc="); Serial.print(client.state()); Serial.println(" try again in 5 seconds"); // Wait 5 seconds before retrying delay(5000); } } } void publisher() { long now = millis(); if (now - lastMsg > 500) { lastMsg = now; long distance=sample(); ++value; //String payload = "{\"dist\" : "; // payload += distance; //payload += "}"; snprintf (msg, 75, "%ld", distance); Serial.print("Publish message: "); Serial.println(msg); mqtt.publish("sonar/distance", msg); } } void topic1_subscriber(String topic, String payload) { Serial.println(s+"Message arrived in function 1 ["+topic+"] "+payload); if ((char)payload[0] == 'F') { leftForward(); } else if ((char)payload[0] == 'S') { leftStop(); } else { leftReverse(); } } void topic2_subscriber(String topic, String payload) { Serial.println(s+"Message arrived in function 2 ["+topic+"] "+payload); if ((char)payload[0] == 'F') { rightForward(); } else if ((char)payload[0] == 'S') { rightStop(); } else { rightReverse(); } } void loop() { client.loop(); threadControl.run(); publisher(); }The publisher reads the sonar distance, send send on the topic "sonar/distance". The inputs are "wheel/Left" and "wheel/Right", values are "F", "S" and "R" for Forward, Stop and Reverse. The robot has no smarts, it just listens for commands, and sends out distance to nearest object in front.
All this information is processed in Node-red. The flow looks like:
Lots of manual controls, mostly individual control of the direction of the wheels. The middle left/right will get both wheels spinning in the same direction or stop. Then the "Bounce Around" function will get the left wheel to turn the car when it gets too close to something. The function looks like:
var d1 = parseInt(msg.payload);var dx = msg.payload;
if (dx < 17) { msg.payload = "R";} else { msg.payload = "F";}//msg.payload.dist = msg.payload.dist;
return msg;
If the distance is less than 17 CM, the left wheel goes in reverse. When the distance is greater than 17CM then the wheel will continue going forward.
It demonstrates when a reliable communications mechanism is there, cloud calculations can be used to do motion controlled systems.
Obviously for a real autonomous car more sensors would be needed to ensure safe travel.
No comments:
Post a Comment