diff --git a/wifi-integrated.ino b/wifi-integrated.ino
index 5261c116e68fcec3367f5bcc9f74ffaf8ea38d31..5de09a457804be25cda1d102b86af4d7d59ea240 100644
--- a/wifi-integrated.ino
+++ b/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");