integrated imu with wifi car

wifi-integrated.ino

+#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