From d28fe10db6d48120d5c4f66b81aae62e35d32d44 Mon Sep 17 00:00:00 2001
From: Reinhard Sprung <reinhard.sprung@gmail.com>
Date: Wed, 17 Oct 2018 00:37:59 +0200
Subject: [PATCH] Robot can now handle multiple wifi networks. If defined,
 robot will create it's own wifi network.

---
 ros_diffdrive_robot.ino | 92 +++++++++++++++++++++--------------------
 1 file changed, 47 insertions(+), 45 deletions(-)

diff --git a/ros_diffdrive_robot.ino b/ros_diffdrive_robot.ino
index 842d6fc..b4ab7de 100644
--- a/ros_diffdrive_robot.ino
+++ b/ros_diffdrive_robot.ino
@@ -36,6 +36,7 @@
 
 #include <math.h>
 #include <ESP8266WiFi.h>
+#include <ESP8266WiFiMulti.h>
 #include <ros.h>
 #include <geometry_msgs/Twist.h>
 
@@ -44,6 +45,14 @@
 #define MAX_PWM 1023  // NOTE: Max PWM value on a NodeMcu is 1023, while on Arduino it's only 255.
 #define MIN_PWM 300   // The min amount of PWM the motors need to move. Depends on the battery, motors and controller.
 
+// Declare functions
+void setupPins();
+void setupSerial();
+void setupWiFi();
+bool rosConnected();
+void onTwist(const geometry_msgs::Twist &msg);
+float mapPwm(float x, float out_min, float out_max);
+
 // Pins
 const uint8_t L_PWM = D1;
 const uint8_t L_BACK = D2;
@@ -52,20 +61,17 @@ const uint8_t R_BACK = D5;
 const uint8_t R_FORW = D6;
 const uint8_t R_PWM = D7;
 
-const char* ssid     = "----SSID----";
-const char* password = "--PASSWORD--";
-
-// Declare functions
-void setupPins();
-void setupSerial();
-void setupWiFi();
-void onTwist(const geometry_msgs::Twist &msg);
+// Wifi
+// If access point is defined, a Wifi network with this name will be created.
+// Remove if you want to connect to an existing network.
+#define ACCESS_POINT_SSID "SMARTCAR"
 
-bool checkRosConnection(bool connected);
-float mapPwm(float x, float out_min, float out_max);
+#ifndef ACCESS_POINT_SSID
+ESP8266WiFiMulti wifi;
+#endif
 
-// ROS
-IPAddress server(192, 168, 0, 12);
+// ROS serial server
+IPAddress server(192, 168, 4, 2);
 ros::NodeHandle node;
 ros::Subscriber<geometry_msgs::Twist> sub("/cmd_vel", &onTwist);
 
@@ -106,17 +112,42 @@ void setupSerial()
 
 void setupWiFi()
 {
-  Serial.print("Connecting to ");
-  Serial.println(ssid);
+#ifdef ACCESS_POINT_SSID
 
-  WiFi.begin(ssid, password);
-  while (WiFi.status() != WL_CONNECTED)
+  WiFi.disconnect();
+  Serial.println("Creating Wifi network");
+  if (WiFi.softAP(ACCESS_POINT_SSID))
+  {
+    Serial.println("Wifi network created");
+    Serial.print("SSID: ");
+    Serial.println(WiFi.softAPSSID());
+    Serial.print("IP:   ");
+    Serial.println(WiFi.softAPIP());
+  }
+
+#else
+
+  WiFi.softAPdisconnect();
+  WiFi.disconnect();
+  WiFi.mode(WIFI_STA);
+  wifi.addAP("--wifi_1--", "--password_1--");
+  wifi.addAP("--wifi_2--", "--password_2--");
+  wifi.addAP("--wifi_3--", "--password_3--");
+
+  Serial.println("Connecting to Wifi");
+  while (wifi.run() != WL_CONNECTED)
   {
     delay(500);
     Serial.print(".");
   }
+
   Serial.println("\nWiFi connected");
+  Serial.print("SSID: ");
+  Serial.println(WiFi.SSID());
+  Serial.print("IP:   ");
   Serial.println(WiFi.localIP());
+
+#endif
 }
 
 void stop()
@@ -137,22 +168,6 @@ void onTwist(const geometry_msgs::Twist &msg)
     return;
   }
 
-  /*
-    Serial.print(msg.linear.x);
-    Serial.print("|");
-    Serial.print(msg.linear.y);
-    Serial.print("|");
-    Serial.print(msg.linear.z);
-    Serial.print("   ");
-
-    Serial.print(msg.angular.x);
-    Serial.print("|");
-    Serial.print(msg.angular.y);
-    Serial.print("|");
-    Serial.print(msg.angular.z);
-    Serial.print("   ");
-  */
-
   // Cap values at [-1 .. 1]
   float x = max(min(msg.linear.x, 1.0f), -1.0f);
   float z = max(min(msg.angular.z, 1.0f), -1.0f);
@@ -166,17 +181,6 @@ void onTwist(const geometry_msgs::Twist &msg)
   uint16_t lPwm = mapPwm(fabs(l), MIN_PWM, MAX_PWM);
   uint16_t rPwm = mapPwm(fabs(r), MIN_PWM, MAX_PWM);
 
-  /*
-    Serial.print(l);
-    Serial.print("|");
-    Serial.print(r);
-    Serial.print("   ");
-
-    Serial.print(lPwm);
-    Serial.print(" | ");
-    Serial.println(rPwm);
-  */
-
   // Set direction pins and PWM
   digitalWrite(L_FORW, l > 0);
   digitalWrite(L_BACK, l < 0);
@@ -193,7 +197,6 @@ void loop()
   node.spinOnce();
 }
 
-
 bool rosConnected()
 {
   // If value changes, notify via LED and console.
@@ -212,4 +215,3 @@ float mapPwm(float x, float out_min, float out_max)
 {
   return x * (out_max - out_min) + out_min;
 }
-