From eb8d925821e3f1c42cf9baf0d8e55fcdfe64f10f Mon Sep 17 00:00:00 2001
From: Muhammad Hazimi Bin Yusri <mhby1g21@soton.ac.uk>
Date: Wed, 26 Mar 2025 11:14:05 +0000
Subject: [PATCH] integrated imu with wifi car
---
wifi-integrated.ino | 629 ++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 629 insertions(+)
create mode 100644 wifi-integrated.ino
diff --git a/wifi-integrated.ino b/wifi-integrated.ino
new file mode 100644
index 0000000..816008d
--- /dev/null
+++ b/wifi-integrated.ino
@@ -0,0 +1,629 @@
+#include <WiFiS3.h>
+#include <Wire.h>
+#include "SparkFun_BMI270_Arduino_Library.h"
+
+// WiFi credentials
+const char ssid[] = "SOTON-IoT";
+const char pass[] = "pUguZXWtBDzl";
+int status = WL_IDLE_STATUS;
+unsigned long lastConnectionAttempt = 0;
+const unsigned long connectionRetryInterval = 1000; // 1 second between reconnection attempts
+
+// Server settings
+WiFiServer server(80);
+WiFiClient client;
+
+// Motor control pins
+// Motor A - Left side
+const int ENA = 10; // PWM pin for left motor speed
+const int IN1 = 9; // Direction control
+const int IN2 = 8; // Direction control
+
+// Motor B - Right side
+const int ENB = 5; // PWM speed control
+const int IN3 = 7; // Direction control
+const int IN4 = 6; // Direction control
+
+// LED indicators
+const int WIFI_LED = 13; // Built-in LED for WiFi status
+const int MOVING_LED = 12; // Optional LED for movement status
+
+// Motor speed settings
+const int SPEED = 200; // Normal speed (0-255)
+const int TURN_SPEED = 180; // Speed during turns
+
+// Movement state tracking
+enum MovementState {
+ STATE_STOP,
+ STATE_FORWARD,
+ STATE_BACKWARD,
+ STATE_LEFT,
+ STATE_RIGHT
+};
+
+MovementState currentState = STATE_STOP;
+unsigned long lastCommandTime = 0;
+const unsigned long commandTimeout = 0; // Disabled by setting to 0
+
+// PID control settings
+float Kp = 1.5; // Proportional gain
+float Ki = 0.1; // Integral gain
+float Kd = 0.05; // Derivative gain
+
+// PID variables
+float error = 0, lastError = 0, integral = 0;
+int Direction = 0;
+int leftSpeed = 0;
+int rightSpeed = 0;
+int baseSpeed = 200; // Base motor speed (0-255)
+
+// IMU setup
+BMI270 imu;
+uint8_t i2cAddress = BMI2_I2C_PRIM_ADDR; // 0x68
+bool imuConnected = false;
+
+// Simple watchdog implementation
+unsigned long lastWatchdogTime = 0;
+const unsigned long watchdogInterval = 1000; // Check every second
+
+// Command queue for reliability
+String lastCommand = "stop";
+bool newCommandReceived = false;
+
+// Connection monitoring
+unsigned long lastConnectionCheck = 0;
+const unsigned long connectionCheckInterval = 5000; // Check connection every 5 seconds
+bool hasValidIP = false;
+
+// IMU usage flag - can be toggled via commands
+bool useIMU = true;
+
+// DNS settings to help with connectivity
+IPAddress dns(8, 8, 8, 8); // Google DNS
+
+void setup() {
+ // Initialize serial
+ Serial.begin(9600);
+ Serial.println("EMG-controlled car with IMU stabilization starting up...");
+
+ // Initialize motor pins
+ pinMode(ENA, OUTPUT);
+ pinMode(IN1, OUTPUT);
+ pinMode(IN2, OUTPUT);
+ pinMode(ENB, OUTPUT);
+ pinMode(IN3, OUTPUT);
+ pinMode(IN4, OUTPUT);
+
+ // Initialize LED indicators
+ pinMode(WIFI_LED, OUTPUT);
+ pinMode(MOVING_LED, OUTPUT);
+
+ // Initialize motors to stopped state
+ stopMotors();
+ digitalWrite(MOVING_LED, LOW);
+
+ // Initialize I2C and IMU
+ Wire.begin();
+
+ // Try to initialize BMI270
+ Serial.println("Connecting to BMI270 IMU...");
+ if(imu.beginI2C(i2cAddress) == BMI2_OK) {
+ Serial.println("BMI270 connected successfully!");
+ // Configure IMU
+ imu.setGyroODR(BMI2_GYR_ODR_100HZ); // 100 Hz update rate
+ imuConnected = true;
+ } else {
+ Serial.println("Warning: BMI270 not connected. Operating without IMU stabilization.");
+ imuConnected = false;
+ useIMU = false;
+ }
+
+ // Connect to WiFi - try multiple times if needed
+ bool connected = false;
+ for (int attempt = 0; attempt < 3 && !connected; attempt++) {
+ Serial.print("WiFi connection attempt #");
+ Serial.println(attempt + 1);
+ connected = connectToWiFi();
+ if (!connected) {
+ delay(5000); // Wait between attempts
+ }
+ }
+
+ // Initialize watchdog time
+ lastWatchdogTime = millis();
+}
+
+void loop() {
+ // Simple software watchdog
+ simpleWatchdog();
+
+ // Check WiFi connection periodically
+ unsigned long currentMillis = millis();
+ if (currentMillis - lastConnectionCheck >= connectionCheckInterval) {
+ lastConnectionCheck = currentMillis;
+ checkWiFiConnection();
+ }
+
+ // Only handle client requests if we have a valid IP
+ if (hasValidIP) {
+ // Check for new HTTP connections
+ handleHTTPClient();
+
+ // Process any new commands
+ if (newCommandReceived) {
+ processCommand(lastCommand);
+ newCommandReceived = false;
+ }
+ }
+
+ // Check for command timeout (safety feature)
+ if (commandTimeout > 0 && currentState != STATE_STOP &&
+ (currentMillis - lastCommandTime >= commandTimeout)) {
+ Serial.println("Command timeout - stopping motors for safety");
+ executeStop();
+ }
+
+ // Apply IMU-based PID control if moving and IMU is enabled
+ if (imuConnected && useIMU && (currentState == STATE_FORWARD || currentState == STATE_BACKWARD)) {
+ applyIMUControl();
+ }
+}
+
+void applyIMUControl() {
+ // Update IMU data
+ imu.getSensorData();
+
+ // Use gyro Z-axis (yaw) for direction control (degrees per second)
+ float gyroZ = imu.data.gyroZ;
+
+ // Calculate error (desired yaw rate = 0 for straight)
+ error = 0 - gyroZ;
+
+ // Update integral (dt = 10ms)
+ integral += error * 0.01;
+ integral = constrain(integral, -50, 50); // Limit windup
+
+ // Calculate derivative
+ float derivative = (error - lastError) / 0.01;
+
+ // PID output
+ float output = Kp * error + Ki * integral + Kd * derivative;
+
+ // Adjust motor speeds based on current state
+ if (currentState == STATE_FORWARD) {
+ Direction = 1;
+ leftSpeed = baseSpeed - output;
+ rightSpeed = baseSpeed + output;
+ } else if (currentState == STATE_BACKWARD) {
+ Direction = -1;
+ leftSpeed = baseSpeed + output;
+ rightSpeed = baseSpeed - output;
+ }
+
+ // Constrain speeds to valid PWM range
+ leftSpeed = constrain(leftSpeed, 0, 255);
+ rightSpeed = constrain(rightSpeed, 0, 255);
+
+ // Apply speeds to motors
+ analogWrite(ENA, leftSpeed);
+ analogWrite(ENB, rightSpeed);
+
+ // Store error for next iteration
+ lastError = error;
+
+ // Small delay for timing consistency
+ delay(10);
+}
+
+void simpleWatchdog() {
+ // Simple software watchdog instead of hardware watchdog
+ unsigned long currentMillis = millis();
+ if (currentMillis - lastWatchdogTime >= watchdogInterval) {
+ lastWatchdogTime = currentMillis;
+
+ // Report current state periodically
+ if (currentState != STATE_STOP) {
+ Serial.print("Currently ");
+ Serial.print(getStateString(currentState));
+ Serial.print(" - IMU stabilization: ");
+ Serial.println(useIMU && imuConnected ? "ON" : "OFF");
+ }
+
+ // Periodically check IP validity if we think we're connected
+ if (WiFi.status() == WL_CONNECTED) {
+ IPAddress ip = WiFi.localIP();
+ if (ip[0] == 0 && ip[1] == 0 && ip[2] == 0 && ip[3] == 0) {
+ hasValidIP = false;
+ Serial.println("WARNING: No valid IP address obtained");
+ } else {
+ if (!hasValidIP) {
+ // Just got a valid IP
+ hasValidIP = true;
+ server.begin();
+ printWiFiStatus();
+ }
+ }
+ }
+ }
+}
+
+bool connectToWiFi() {
+ // Check for WiFi module
+ if (WiFi.status() == WL_NO_MODULE) {
+ Serial.println("Communication with WiFi module failed!");
+ // Blink error code on LED
+ blinkLED(WIFI_LED, 5, 500);
+ return false;
+ }
+
+ // Print firmware version
+ String fv = WiFi.firmwareVersion();
+ Serial.print("Firmware version: ");
+ Serial.println(fv);
+
+ // Set WiFi LED to indicate connecting
+ digitalWrite(WIFI_LED, HIGH);
+
+ // Disconnect if already connected
+ WiFi.disconnect();
+ delay(1000);
+
+ // Configure WiFi to use Google DNS to help with IP acquisition
+ WiFi.setDNS(dns);
+
+ // Attempt to connect to WiFi network
+ Serial.print("Attempting to connect to Network: ");
+ Serial.println(ssid);
+
+ status = WiFi.begin(ssid, pass);
+
+ // Wait for connection or timeout
+ int attempts = 0;
+ while (status != WL_CONNECTED && attempts < 30) { // 15 seconds timeout
+ delay(500);
+ Serial.print(".");
+ attempts++;
+ status = WiFi.status();
+ digitalWrite(WIFI_LED, !digitalRead(WIFI_LED)); // Blink while connecting
+ }
+
+ if (status == WL_CONNECTED) {
+ Serial.println("\nConnected to WiFi");
+ digitalWrite(WIFI_LED, HIGH); // Solid on for connected
+
+ // Check for valid IP address
+ IPAddress ip = WiFi.localIP();
+
+ if (ip[0] == 0 && ip[1] == 0 && ip[2] == 0 && ip[3] == 0) {
+ Serial.println("Connected to WiFi but no valid IP address obtained. Will retry...");
+ digitalWrite(WIFI_LED, LOW);
+ hasValidIP = false;
+ return false;
+ }
+
+ hasValidIP = true;
+ printWiFiStatus();
+
+ server.begin();
+ Serial.println("EMG-controlled car server started");
+ return true;
+ } else {
+ Serial.println("\nFailed to connect to WiFi");
+ digitalWrite(WIFI_LED, LOW);
+ hasValidIP = false;
+ return false;
+ }
+
+ lastConnectionAttempt = millis();
+}
+
+void checkWiFiConnection() {
+ if (WiFi.status() != WL_CONNECTED || !hasValidIP) {
+ digitalWrite(WIFI_LED, LOW);
+ if (WiFi.status() != WL_CONNECTED) {
+ Serial.println("WiFi connection lost!");
+ } else {
+ Serial.println("Connected but no valid IP address!");
+ }
+
+ // Only try reconnecting if enough time has passed since last attempt
+ unsigned long currentMillis = millis();
+ if (currentMillis - lastConnectionAttempt >= connectionRetryInterval) {
+ connectToWiFi();
+ lastConnectionAttempt = currentMillis;
+ }
+ } else {
+ digitalWrite(WIFI_LED, HIGH); // Ensure LED is on when connected
+
+ // Double-check IP validity
+ IPAddress ip = WiFi.localIP();
+ if (ip[0] == 0 && ip[1] == 0 && ip[2] == 0 && ip[3] == 0) {
+ hasValidIP = false;
+ Serial.println("IP address became invalid!");
+ }
+ }
+}
+
+void handleHTTPClient() {
+ // Check if client has connected
+ client = server.available();
+
+ if (client) {
+ Serial.println("New client connected");
+
+ // HTTP request buffer
+ String currentLine = "";
+ String httpRequest = "";
+ boolean currentLineIsBlank = true;
+ unsigned long clientConnectTime = millis();
+
+ // Process request while client is connected or until timeout
+ while (client.connected() && (millis() - clientConnectTime < 3000)) {
+ if (client.available()) {
+ char c = client.read();
+ httpRequest += c;
+
+ // End of HTTP request line
+ if (c == '\n' && currentLineIsBlank) {
+ // Send HTTP response
+ sendHTTPResponse();
+ break;
+ }
+
+ // End of a line
+ if (c == '\n') {
+ currentLineIsBlank = true;
+ currentLine = "";
+ }
+ else if (c != '\r') {
+ // Character in current line
+ currentLineIsBlank = false;
+ currentLine += c;
+ }
+ }
+ }
+
+ // Parse command from HTTP request
+ if (httpRequest.indexOf("GET /") != -1) {
+ parseCommand(httpRequest);
+ }
+
+ // Allow time for data to be sent
+ delay(10);
+
+ // Close the connection
+ client.stop();
+ Serial.println("Client disconnected");
+ }
+}
+
+void sendHTTPResponse() {
+ String response = "HTTP/1.1 200 OK\r\n";
+ response += "Content-Type: application/json\r\n";
+ response += "Connection: close\r\n\r\n";
+
+ // Create JSON response with current status
+ response += "{\r\n";
+ response += " \"status\": \"ok\",\r\n";
+ response += " \"movement\": \"" + getStateString(currentState) + "\",\r\n";
+ response += " \"imu_enabled\": " + String((useIMU && imuConnected) ? "true" : "false") + ",\r\n";
+ response += " \"command_received\": true,\r\n";
+ response += " \"ip\": \"" + WiFi.localIP().toString() + "\"\r\n";
+ response += "}\r\n";
+
+ client.print(response);
+}
+
+String getStateString(MovementState state) {
+ switch(state) {
+ case STATE_STOP: return "stopped";
+ case STATE_FORWARD: return "forward";
+ case STATE_BACKWARD: return "backward";
+ case STATE_LEFT: return "left";
+ case STATE_RIGHT: return "right";
+ default: return "unknown";
+ }
+}
+
+void parseCommand(String request) {
+ int start = request.indexOf("GET /") + 5;
+ int end = request.indexOf(" HTTP");
+
+ if (start != -1 && end != -1) {
+ String command = request.substring(start, end);
+ Serial.println("Received command: " + command);
+
+ // Queue the command for processing
+ lastCommand = command;
+ newCommandReceived = true;
+ }
+}
+
+void processCommand(String command) {
+ // Update last command time for timeout monitoring
+ lastCommandTime = millis();
+
+ // Process the command
+ if (command == "forward") {
+ executeForward();
+ }
+ else if (command == "backward") {
+ executeBackward();
+ }
+ else if (command == "left") {
+ executeLeft();
+ }
+ else if (command == "right") {
+ executeRight();
+ }
+ else if (command == "stop") {
+ executeStop();
+ }
+ else if (command == "imu_on") {
+ // Enable IMU stabilization if available
+ if (imuConnected) {
+ useIMU = true;
+ Serial.println("IMU stabilization enabled");
+ } else {
+ Serial.println("IMU not connected, cannot enable stabilization");
+ }
+ }
+ else if (command == "imu_off") {
+ // Disable IMU stabilization
+ useIMU = false;
+ Serial.println("IMU stabilization disabled");
+ }
+ else if (command == "ping") {
+ // Just for connection testing
+ Serial.println("Ping received - connection test successful");
+ }
+ else if (command == "reconnect") {
+ // Force WiFi reconnection
+ Serial.println("Reconnect command received - restarting WiFi");
+ hasValidIP = false;
+ connectToWiFi();
+ }
+ else {
+ Serial.println("Unknown command: " + command);
+ }
+}
+
+void executeForward() {
+ Serial.println("Moving forward" + String((useIMU && imuConnected) ? " with IMU stabilization" : ""));
+
+ // Reset PID variables
+ error = 0;
+ lastError = 0;
+ integral = 0;
+
+ // Motor A forward
+ digitalWrite(IN1, HIGH);
+ digitalWrite(IN2, LOW);
+
+ // Motor B forward
+ digitalWrite(IN3, HIGH);
+ digitalWrite(IN4, LOW);
+
+ // Set initial motor speeds
+ analogWrite(ENA, SPEED);
+ analogWrite(ENB, SPEED);
+
+ currentState = STATE_FORWARD;
+ digitalWrite(MOVING_LED, HIGH);
+}
+
+void executeBackward() {
+ Serial.println("Moving backward" + String((useIMU && imuConnected) ? " with IMU stabilization" : ""));
+
+ // Reset PID variables
+ error = 0;
+ lastError = 0;
+ integral = 0;
+
+ // Motor A backward
+ digitalWrite(IN1, LOW);
+ digitalWrite(IN2, HIGH);
+
+ // Motor B backward
+ digitalWrite(IN3, LOW);
+ digitalWrite(IN4, HIGH);
+
+ // Set initial motor speeds
+ analogWrite(ENA, SPEED);
+ analogWrite(ENB, SPEED);
+
+ currentState = STATE_BACKWARD;
+ digitalWrite(MOVING_LED, HIGH);
+}
+
+void executeLeft() {
+ Serial.println("Turning left");
+
+ // Motor A forward
+ digitalWrite(IN1, HIGH);
+ digitalWrite(IN2, LOW);
+ analogWrite(ENA, SPEED);
+
+ // Motor B backward for sharp turn
+ digitalWrite(IN3, LOW);
+ digitalWrite(IN4, HIGH);
+ analogWrite(ENB, TURN_SPEED);
+
+ currentState = STATE_LEFT;
+ digitalWrite(MOVING_LED, HIGH);
+}
+
+void executeRight() {
+ Serial.println("Turning right");
+
+ // Motor A backward for sharp turn
+ digitalWrite(IN1, LOW);
+ digitalWrite(IN2, HIGH);
+ analogWrite(ENA, TURN_SPEED);
+
+ // Motor B forward
+ digitalWrite(IN3, HIGH);
+ digitalWrite(IN4, LOW);
+ analogWrite(ENB, SPEED);
+
+ currentState = STATE_RIGHT;
+ digitalWrite(MOVING_LED, HIGH);
+}
+
+void executeStop() {
+ Serial.println("Stopping motors");
+
+ // Stop both motors
+ digitalWrite(IN1, LOW);
+ digitalWrite(IN2, LOW);
+ analogWrite(ENA, 0);
+
+ digitalWrite(IN3, LOW);
+ digitalWrite(IN4, LOW);
+ analogWrite(ENB, 0);
+
+ currentState = STATE_STOP;
+ digitalWrite(MOVING_LED, LOW);
+}
+
+void stopMotors() {
+ // Stop both motors immediately (for initialization or emergencies)
+ digitalWrite(IN1, LOW);
+ digitalWrite(IN2, LOW);
+ analogWrite(ENA, 0);
+
+ digitalWrite(IN3, LOW);
+ digitalWrite(IN4, LOW);
+ analogWrite(ENB, 0);
+
+ currentState = STATE_STOP;
+}
+
+void blinkLED(int pin, int times, int delayMs) {
+ for (int i = 0; i < times; i++) {
+ digitalWrite(pin, HIGH);
+ delay(delayMs);
+ digitalWrite(pin, LOW);
+ delay(delayMs);
+ }
+}
+
+void printWiFiStatus() {
+ Serial.print("SSID: ");
+ Serial.println(WiFi.SSID());
+
+ IPAddress ip = WiFi.localIP();
+ Serial.print("IP Address: ");
+ Serial.println(ip);
+
+ long rssi = WiFi.RSSI();
+ Serial.print("Signal strength (RSSI): ");
+ Serial.print(rssi);
+ Serial.println(" dBm");
+
+ Serial.println("To control this car, send HTTP requests to:");
+ Serial.print("http://");
+ Serial.print(ip);
+ Serial.println("/command");
+ Serial.println("Where command is: forward, backward, left, right, stop, imu_on, imu_off, or reconnect");
+}
\ No newline at end of file
--
GitLab