fixed imu but forward backward reversed

wifi-integrated.ino

@@ -43,9 +43,9 @@ enum MovementState {
 
 MovementState currentState = STATE_STOP;
 unsigned long lastCommandTime = 0;
-const unsigned long commandTimeout = 0; // Disabled by setting to 0
+const unsigned long commandTimeout = 0; // Disabled because not working
 
-// PID control settings
+// PID constants (tune these) - Matching Medical_V2.ino
 float Kp = 1.5;  // Proportional gain
 float Ki = 0.1;  // Integral gain
 float Kd = 0.05; // Derivative gain
@@ -57,7 +57,7 @@ int leftSpeed = 0;
 int rightSpeed = 0;
 int baseSpeed = 200; // Base motor speed (0-255)
 
-// IMU setup
+// BMI270 IMU object
 BMI270 imu;
 uint8_t i2cAddress = BMI2_I2C_PRIM_ADDR; // 0x68
 bool imuConnected = false;
@@ -84,7 +84,7 @@ 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...");
+  Serial.println("EMG-controlled wheelchair with IMU stabilization starting up...");
   
   // Initialize motor pins
   pinMode(ENA, OUTPUT);
@@ -156,20 +156,99 @@ void loop() {
     }
   }
   
+  // Apply movement control based on current state
+  switch(currentState) {
+    case STATE_FORWARD:
+      moveForward();
+      break;
+    case STATE_BACKWARD:
+      moveBackward();
+      break;
+    case STATE_LEFT:
+      turnLeft();
+      break;
+    case STATE_RIGHT:
+      turnRight();
+      break;
+    case STATE_STOP:
+      // Motors already stopped
+      break;
+  }
+  
   // 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();
+}
+
+// Move Forward - Using the exact structure from Medical_V2.ino
+void moveForward() {
+  // Set direction pins first
+  digitalWrite(IN2, HIGH);
+  digitalWrite(IN1, LOW);
+  digitalWrite(IN4, HIGH);
+  digitalWrite(IN3, LOW);
+  Direction = 1;
+
+  // Then apply IMU control if available
+  if (imuConnected && useIMU) {
+    IMU_Control();
+  } else {
+    // Without IMU, just use basic speed control
+    analogWrite(ENA, SPEED);
+    analogWrite(ENB, SPEED);
   }
 }
 
-void applyIMUControl() {
+// Move Backwards - Using the exact structure from Medical_V2.ino
+void moveBackward() {
+  // Set direction pins first
+  digitalWrite(IN2, LOW);
+  digitalWrite(IN1, HIGH);
+  digitalWrite(IN4, LOW);
+  digitalWrite(IN3, HIGH);
+  Direction = -1;
+
+  // Then apply IMU control if available
+  if (imuConnected && useIMU) {
+    IMU_Control();
+  } else {
+    // Without IMU, just use basic speed control
+    analogWrite(ENA, SPEED);
+    analogWrite(ENB, SPEED);
+  }
+}
+
+// Turn Left - Using the exact structure from Medical_V2.ino
+void turnLeft() {
+  digitalWrite(IN2, HIGH);
+  digitalWrite(IN1, LOW);
+  digitalWrite(IN4, LOW);
+  digitalWrite(IN3, LOW);
+  Direction = 0;
+  
+  // No IMU control for turning
+  analogWrite(ENA, SPEED);
+  analogWrite(ENB, 0);
+}
+
+// Turn Right - Using the exact structure from Medical_V2.ino
+void turnRight() {
+  digitalWrite(IN2, LOW);
+  digitalWrite(IN1, LOW);
+  digitalWrite(IN4, HIGH);
+  digitalWrite(IN3, LOW);
+  Direction = 0;
+  
+  // No IMU control for turning
+  analogWrite(ENA, 0);
+  analogWrite(ENB, SPEED);
+}
+
+// Using the exact IMU_Control from Medical_V2.ino
+void IMU_Control() {
   // Update IMU data
   imu.getSensorData();
 
@@ -189,13 +268,12 @@ void applyIMUControl() {
   // PID output
   float output = Kp * error + Ki * integral + Kd * derivative;
 
-  // Adjust motor speeds based on current state
-  if (currentState == STATE_FORWARD) {
-    Direction = 1;
+  // Adjust motor speeds based on direction
+  if(Direction == 1){ //Moving Forward
     leftSpeed = baseSpeed - output;
     rightSpeed = baseSpeed + output;
-  } else if (currentState == STATE_BACKWARD) {
-    Direction = -1;
+  }
+  else if(Direction == -1){ //Moving Backward
     leftSpeed = baseSpeed + output;
     rightSpeed = baseSpeed - output;
   }
@@ -305,7 +383,7 @@ bool connectToWiFi() {
     printWiFiStatus();
     
     server.begin();
-    Serial.println("EMG-controlled car server started");
+    Serial.println("EMG-controlled wheelchair server started");
     return true;
   } else {
     Serial.println("\nFailed to connect to WiFi");
@@ -496,18 +574,6 @@ void executeForward() {
   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);
 }
@@ -520,74 +586,31 @@ void executeBackward() {
   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);
-  
+  stopMotors();
   currentState = STATE_STOP;
   digitalWrite(MOVING_LED, LOW);
 }
 
 void stopMotors() {
-  // Stop both motors immediately (for initialization or emergencies)
+  // Stop both motors immediately
   digitalWrite(IN1, LOW);
   digitalWrite(IN2, LOW);
   analogWrite(ENA, 0);
@@ -595,8 +618,6 @@ void stopMotors() {
   digitalWrite(IN3, LOW);
   digitalWrite(IN4, LOW);
   analogWrite(ENB, 0);
-  
-  currentState = STATE_STOP;
 }
 
 void blinkLED(int pin, int times, int delayMs) {
@@ -621,7 +642,7 @@ void printWiFiStatus() {
   Serial.print(rssi);
   Serial.println(" dBm");
   
-  Serial.println("To control this car, send HTTP requests to:");
+  Serial.println("To control this wheelchair, send HTTP requests to:");
   Serial.print("http://");
   Serial.print(ip);
   Serial.println("/command");