Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
A
Arduino Codes
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Package Registry
Model registry
Operate
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Medics
Arduino Codes
Commits
7acd6de3
Commit
7acd6de3
authored
1 month ago
by
mhby1g21
Browse files
Options
Downloads
Patches
Plain Diff
added wifi implementation
parent
f2fa9cb1
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
Medical_V2.ino
+176
-0
176 additions, 0 deletions
Medical_V2.ino
serial/serial.ino
+194
-116
194 additions, 116 deletions
serial/serial.ino
wifi-car.ino
+452
-0
452 additions, 0 deletions
wifi-car.ino
with
822 additions
and
116 deletions
Medical_V2.ino
0 → 100644
+
176
−
0
View file @
7acd6de3
//Wheelchair motion with simple PID control system
//DO NOT SWITCH ON THE POWER BUTTON ON THE CHASIS IF THE ARDUINO IS POWERED ON WITH USB C!!!
#include
<Wire.h>
#include
"SparkFun_BMI270_Arduino_Library.h"
// L298N motor driver pins
#define ENA 9 // PWM pin for left motor speed
#define IN1 8 // Left motor direction 1
#define IN2 7 // Left motor direction 2
#define IN3 5 // Right motor direction 1
#define IN4 4 // Right motor direction 2
#define ENB 3 // PWM pin for right motor speed
// PID constants (tune these)
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)
// BMI270 IMU object
BMI270
imu
;
uint8_t
i2cAddress
=
BMI2_I2C_PRIM_ADDR
;
// 0x68
void
setup
()
{
// Initialize serial for debugging
Serial
.
begin
(
115200
);
// Set motor pins as outputs
pinMode
(
ENA
,
OUTPUT
);
pinMode
(
ENB
,
OUTPUT
);
pinMode
(
IN1
,
OUTPUT
);
pinMode
(
IN2
,
OUTPUT
);
pinMode
(
IN3
,
OUTPUT
);
pinMode
(
IN4
,
OUTPUT
);
// Initialize I2C
Wire
.
begin
();
// Initialize BMI270 (default I2C address is 0x68 or 0x69, depending on SDO pin)
while
(
imu
.
beginI2C
(
i2cAddress
)
!=
BMI2_OK
)
{
// Not connected, inform user
Serial
.
println
(
"Error: BMI270 not connected, check wiring and I2C address!"
);
// Wait a bit to see if connection is established
delay
(
1000
);
}
Serial
.
println
(
"BMI270 connected!"
);
// Configure IMU (default settings are usually fine)
imu
.
setGyroODR
(
BMI2_GYR_ODR_100HZ
);
// 100 Hz update rate
// Initialize motor to stop mode
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN2
,
LOW
);
digitalWrite
(
IN3
,
LOW
);
digitalWrite
(
IN4
,
LOW
);
// Set zero Motor Speed
analogWrite
(
ENA
,
0
);
analogWrite
(
ENB
,
0
);
}
void
loop
()
{
MoveForward
();
// MoveBackward();
}
//Move Forward
void
MoveForward
()
{
digitalWrite
(
IN2
,
HIGH
);
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN4
,
HIGH
);
digitalWrite
(
IN3
,
LOW
);
Direction
=
1
;
//check error to make sure moving in a straight line
IMU_Control
();
}
//Move Backwards
void
MoveBackward
()
{
digitalWrite
(
IN2
,
LOW
);
digitalWrite
(
IN1
,
HIGH
);
digitalWrite
(
IN4
,
LOW
);
digitalWrite
(
IN3
,
HIGH
);
Direction
=
-
1
;
//check error to make sure moving in a straight line
IMU_Control
();
}
//Turn Left
void
TurnLeft
()
{
digitalWrite
(
IN2
,
HIGH
);
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN4
,
LOW
);
digitalWrite
(
IN3
,
LOW
);
Direction
=
0
;
}
//Turn Right
void
TurnRight
()
{
digitalWrite
(
IN2
,
LOW
);
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN4
,
HIGH
);
digitalWrite
(
IN3
,
LOW
);
Direction
=
0
;
}
//Stop Moving
void
Stop
()
{
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN2
,
LOW
);
digitalWrite
(
IN3
,
LOW
);
digitalWrite
(
IN4
,
LOW
);
Direction
=
0
;
}
void
IMU_Control
()
{
// 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
if
(
Direction
==
1
){
//Moving Forward
leftSpeed
=
baseSpeed
-
output
;
rightSpeed
=
baseSpeed
+
output
;
}
else
if
(
Direction
==
-
1
){
//Moving Backward
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
);
// Debugging output (optional)
// Serial.print("GyroZ: "); Serial.print(gyroZ);
// Serial.print(" | Left: "); Serial.print(leftSpeed);
// Serial.print(" | Right: "); Serial.println(rightSpeed);
// Store error for next iteration
lastError
=
error
;
// Small delay for timing consistency
delay
(
10
);
}
\ No newline at end of file
This diff is collapsed.
Click to expand it.
serial/serial.ino
+
194
−
116
View file @
7acd6de3
#include
<Servo.h>
// Create servo objects
Servo
index_finger
;
Servo
middle_finger
;
Servo
ring_finger
;
Servo
pinky_finger
;
// Servo pins - adjust according to your Arduino connections
const
int
INDEX_PIN
=
2
;
const
int
MIDDLE_PIN
=
3
;
const
int
RING_PIN
=
4
;
const
int
PINKY_PIN
=
5
;
// Current positions of each finger
int
index_pos
=
90
;
int
middle_pos
=
90
;
int
ring_pos
=
90
;
int
pinky_pos
=
90
;
// Buffer for incoming serial data
String
inputBuffer
=
""
;
bool
commandReady
=
false
;
#include
<WiFiS3.h>
// WiFi credentials
const
char
ssid
[]
=
"SOTON-IoT"
;
const
char
pass
[]
=
"pUguZXWtBDzl"
;
int
status
=
WL_IDLE_STATUS
;
// Server settings
WiFiServer
server
(
80
);
// Motor control pins
// Motor A - Left side
const
int
ENA
=
10
;
// PWM speed control
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
// Motor speed (0-255)
const
int
SPEED
=
200
;
const
int
TURN_SPEED
=
180
;
void
setup
()
{
// Initialize serial communication at 9600 bps
Serial
.
begin
(
9600
);
while
(
!
Serial
)
{
;
// Wait for serial port to connect (needed for native USB)
}
// Attach servos to their pins
index_finger
.
attach
(
INDEX_PIN
);
middle_finger
.
attach
(
MIDDLE_PIN
);
ring_finger
.
attach
(
RING_PIN
);
pinky_finger
.
attach
(
PINKY_PIN
);
// Initialize motor pins
pinMode
(
ENA
,
OUTPUT
);
pinMode
(
IN1
,
OUTPUT
);
pinMode
(
IN2
,
OUTPUT
);
pinMode
(
ENB
,
OUTPUT
);
pinMode
(
IN3
,
OUTPUT
);
pinMode
(
IN4
,
OUTPUT
);
// Initialize all fingers to neutral position
index_finger
.
write
(
index_pos
);
middle_finger
.
write
(
middle_pos
);
ring_finger
.
write
(
ring_pos
);
pinky_finger
.
write
(
pinky_pos
);
// Initialize motors to stopped state
stopMotors
();
// Show startup message
Serial
.
println
(
"InMoov Hand Controller Ready"
);
Serial
.
println
(
"Format: [finger]:[angle]"
);
Serial
.
println
(
"Example: index:180"
);
// Check for WiFi module
if
(
WiFi
.
status
()
==
WL_NO_MODULE
)
{
Serial
.
println
(
"Communication with WiFi module failed!"
);
while
(
true
);
}
// Check firmware version
String
fv
=
WiFi
.
firmwareVersion
();
Serial
.
print
(
"Firmware version: "
);
Serial
.
println
(
fv
);
// Attempt to connect to WiFi network
while
(
status
!=
WL_CONNECTED
)
{
Serial
.
print
(
"Attempting to connect to Network: "
);
Serial
.
println
(
ssid
);
status
=
WiFi
.
begin
(
ssid
,
pass
);
delay
(
10000
);
}
Serial
.
println
(
"Connected to WiFi"
);
printWiFiStatus
();
server
.
begin
();
Serial
.
println
(
"EMG-controlled car server started"
);
}
void
loop
()
{
// Check for incoming serial data
while
(
Serial
.
available
()
>
0
)
{
// Read incoming byte
char
incomingByte
=
Serial
.
read
();
WiFiClient
client
=
server
.
available
();
if
(
client
)
{
Serial
.
println
(
"New client connected"
);
String
request
=
""
;
boolean
currentLineIsBlank
=
true
;
// Add to buffer if not end of line
if
(
incomingByte
!=
'\n'
)
{
inputBuffer
+=
incomingByte
;
}
else
{
// End of line, set flag to process command
commandReady
=
true
;
while
(
client
.
connected
())
{
if
(
client
.
available
())
{
char
c
=
client
.
read
();
request
+=
c
;
if
(
c
==
'\n'
&&
currentLineIsBlank
)
{
// Send HTTP response
client
.
println
(
"HTTP/1.1 200 OK"
);
client
.
println
(
"Content-Type: text/plain"
);
client
.
println
(
"Connection: close"
);
client
.
println
();
client
.
println
(
"Command received"
);
break
;
}
if
(
c
==
'\n'
)
{
currentLineIsBlank
=
true
;
}
else
if
(
c
!=
'\r'
)
{
currentLineIsBlank
=
false
;
}
}
}
}
// Process command if ready
if
(
commandReady
)
{
Serial
.
print
(
"Received command: "
);
Serial
.
println
(
inputBuffer
);
processCommand
(
inputBuffer
);
// Process the request
if
(
request
.
indexOf
(
"GET /"
)
!=
-
1
)
{
processRequest
(
request
);
}
// Clear buffer and reset flag
inputBuffer
=
""
;
commandReady
=
false
;
delay
(
10
);
client
.
stop
()
;
Serial
.
println
(
"Client disconnected"
)
;
}
// Small delay to avoid hogging CPU
delay
(
10
);
}
void
process
Command
(
String
command
)
{
// Parse command in format: "finger:angle"
int
colonIndex
=
command
.
indexOf
(
':'
);
void
process
Request
(
String
request
)
{
int
start
=
request
.
indexOf
(
"GET "
)
+
4
;
int
end
=
request
.
indexOf
(
" HTTP"
);
if
(
colonIndex
!=
-
1
)
{
String
finger
=
command
.
substring
(
0
,
colonIndex
);
String
angleStr
=
command
.
substring
(
colonIndex
+
1
);
int
angle
=
angleStr
.
toInt
();
if
(
start
!=
-
1
&&
end
!=
-
1
)
{
String
command
=
request
.
substring
(
start
,
end
);
Serial
.
println
(
"Command: "
+
command
);
// Constrain angle to valid range
angle
=
constrain
(
angle
,
30
,
150
);
// Update finger position based on command
if
(
finger
==
"index"
)
{
index_pos
=
angle
;
index_finger
.
write
(
index_pos
);
Serial
.
print
(
"Index finger moved to: "
);
Serial
.
println
(
angle
);
// Parse command and control motors
if
(
command
==
"/forward"
)
{
moveForward
();
}
else
if
(
finger
==
"middle"
)
{
middle_pos
=
angle
;
middle_finger
.
write
(
middle_pos
);
Serial
.
print
(
"Middle finger moved to: "
);
Serial
.
println
(
angle
);
else
if
(
command
==
"/backward"
)
{
moveBackward
();
}
else
if
(
finger
==
"ring"
)
{
ring_pos
=
angle
;
ring_finger
.
write
(
ring_pos
);
Serial
.
print
(
"Ring finger moved to: "
);
Serial
.
println
(
angle
);
else
if
(
command
==
"/left"
)
{
turnLeft
();
}
else
if
(
finger
==
"pinky"
)
{
pinky_pos
=
angle
;
pinky_finger
.
write
(
pinky_pos
);
Serial
.
print
(
"Pinky finger moved to: "
);
Serial
.
println
(
angle
);
else
if
(
command
==
"/right"
)
{
turnRight
();
}
else
if
(
finger
==
"all"
)
{
// Move all fingers to the same position
index_pos
=
angle
;
middle_pos
=
angle
;
ring_pos
=
angle
;
pinky_pos
=
angle
;
index_finger
.
write
(
index_pos
);
middle_finger
.
write
(
middle_pos
);
ring_finger
.
write
(
ring_pos
);
pinky_finger
.
write
(
pinky_pos
);
Serial
.
print
(
"All fingers moved to: "
);
Serial
.
println
(
angle
);
else
if
(
command
==
"/stop"
)
{
stopMotors
();
}
else
{
Serial
.
print
(
"Unknown finger command: "
);
Serial
.
println
(
f
ing
er
);
else
if
(
command
==
"/ping"
)
{
// Just for connection testing
Serial
.
println
(
"P
ing
received"
);
}
}
else
{
Serial
.
print
(
"Invalid command format. Received: "
);
Serial
.
println
(
command
);
Serial
.
println
(
"Format should be: finger:angle"
);
}
}
void
moveForward
()
{
Serial
.
println
(
"Moving forward"
);
// Motor A forward
digitalWrite
(
IN1
,
HIGH
);
digitalWrite
(
IN2
,
LOW
);
analogWrite
(
ENA
,
SPEED
);
// Motor B forward
digitalWrite
(
IN3
,
HIGH
);
digitalWrite
(
IN4
,
LOW
);
analogWrite
(
ENB
,
SPEED
);
}
void
moveBackward
()
{
Serial
.
println
(
"Moving backward"
);
// Motor A backward
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN2
,
HIGH
);
analogWrite
(
ENA
,
SPEED
);
// Motor B backward
digitalWrite
(
IN3
,
LOW
);
digitalWrite
(
IN4
,
HIGH
);
analogWrite
(
ENB
,
SPEED
);
}
void
turnLeft
()
{
Serial
.
println
(
"Turning left"
);
// Motor A forward faster
digitalWrite
(
IN1
,
HIGH
);
digitalWrite
(
IN2
,
LOW
);
analogWrite
(
ENA
,
SPEED
);
// Motor B slower or reverse for sharper turn
digitalWrite
(
IN3
,
LOW
);
digitalWrite
(
IN4
,
HIGH
);
analogWrite
(
ENB
,
TURN_SPEED
);
}
void
turnRight
()
{
Serial
.
println
(
"Turning right"
);
// Motor A slower or reverse for sharper turn
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN2
,
HIGH
);
analogWrite
(
ENA
,
TURN_SPEED
);
// Motor B forward faster
digitalWrite
(
IN3
,
HIGH
);
digitalWrite
(
IN4
,
LOW
);
analogWrite
(
ENB
,
SPEED
);
}
void
stopMotors
()
{
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
);
}
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, or stop"
);
}
\ No newline at end of file
This diff is collapsed.
Click to expand it.
wifi-car.ino
0 → 100644
+
452
−
0
View file @
7acd6de3
#include
<WiFiS3.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
=
10000
;
// 10 seconds between reconnection attempts
// Server settings
WiFiServer
server
(
80
);
WiFiClient
client
;
// Motor control pins
// Motor A - Left side
const
int
ENA
=
10
;
// PWM speed control
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 (optional - connect if available)
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
=
10000
;
// 10 seconds timeout for safety
// 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
void
setup
()
{
// Initialize serial
Serial
.
begin
(
9600
);
Serial
.
println
(
"EMG-controlled car 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
);
// Connect to WiFi
connectToWiFi
();
// 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
();
}
// Check for new HTTP connections
handleHTTPClient
();
// Process any new commands
if
(
newCommandReceived
)
{
processCommand
(
lastCommand
);
newCommandReceived
=
false
;
}
// Check for command timeout (safety feature)
if
(
currentState
!=
STATE_STOP
&&
(
currentMillis
-
lastCommandTime
>=
commandTimeout
))
{
Serial
.
println
(
"Command timeout - stopping motors for safety"
);
executeStop
();
}
}
void
simpleWatchdog
()
{
// Simple software watchdog instead of hardware watchdog
unsigned
long
currentMillis
=
millis
();
if
(
currentMillis
-
lastWatchdogTime
>=
watchdogInterval
)
{
lastWatchdogTime
=
currentMillis
;
// Perform any critical checks here
// This is a good place to verify system state
}
}
void
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
);
while
(
true
);
// Cannot continue without WiFi module
}
// Print firmware version
String
fv
=
WiFi
.
firmwareVersion
();
Serial
.
print
(
"Firmware version: "
);
Serial
.
println
(
fv
);
// Set WiFi LED to indicate connecting
digitalWrite
(
WIFI_LED
,
HIGH
);
// 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
<
20
)
{
delay
(
500
);
Serial
.
print
(
"."
);
attempts
++
;
status
=
WiFi
.
status
();
digitalWrite
(
WIFI_LED
,
!
digitalRead
(
WIFI_LED
));
// Blink while connecting
}
if
(
status
==
WL_CONNECTED
)
{
Serial
.
println
(
"
\n
Connected to WiFi"
);
digitalWrite
(
WIFI_LED
,
HIGH
);
// Solid on for connected
printWiFiStatus
();
server
.
begin
();
Serial
.
println
(
"EMG-controlled car server started"
);
}
else
{
Serial
.
println
(
"
\n
Failed to connect to WiFi"
);
digitalWrite
(
WIFI_LED
,
LOW
);
}
lastConnectionAttempt
=
millis
();
}
void
checkWiFiConnection
()
{
if
(
WiFi
.
status
()
!=
WL_CONNECTED
)
{
digitalWrite
(
WIFI_LED
,
LOW
);
Serial
.
println
(
"WiFi connection lost!"
);
// Only try reconnecting if enough time has passed since last attempt
unsigned
long
currentMillis
=
millis
();
if
(
currentMillis
-
lastConnectionAttempt
>=
connectionRetryInterval
)
{
Serial
.
println
(
"Attempting to reconnect..."
);
WiFi
.
disconnect
();
delay
(
1000
);
status
=
WiFi
.
begin
(
ssid
,
pass
);
// Wait briefly for connection
delay
(
5000
);
if
(
WiFi
.
status
()
==
WL_CONNECTED
)
{
Serial
.
println
(
"Successfully reconnected to WiFi"
);
digitalWrite
(
WIFI_LED
,
HIGH
);
server
.
begin
();
}
else
{
Serial
.
println
(
"Reconnection failed"
);
}
lastConnectionAttempt
=
currentMillis
;
}
}
else
{
digitalWrite
(
WIFI_LED
,
HIGH
);
// Ensure LED is on when connected
}
}
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
+=
"
\"
command_received
\"
: true
\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
==
"ping"
)
{
// Just for connection testing
Serial
.
println
(
"Ping received - connection test successful"
);
}
else
{
Serial
.
println
(
"Unknown command: "
+
command
);
}
}
void
executeForward
()
{
if
(
currentState
!=
STATE_FORWARD
)
{
Serial
.
println
(
"Moving forward"
);
// Motor A forward
digitalWrite
(
IN1
,
HIGH
);
digitalWrite
(
IN2
,
LOW
);
analogWrite
(
ENA
,
SPEED
);
// Motor B forward
digitalWrite
(
IN3
,
HIGH
);
digitalWrite
(
IN4
,
LOW
);
analogWrite
(
ENB
,
SPEED
);
currentState
=
STATE_FORWARD
;
digitalWrite
(
MOVING_LED
,
HIGH
);
}
}
void
executeBackward
()
{
if
(
currentState
!=
STATE_BACKWARD
)
{
Serial
.
println
(
"Moving backward"
);
// Motor A backward
digitalWrite
(
IN1
,
LOW
);
digitalWrite
(
IN2
,
HIGH
);
analogWrite
(
ENA
,
SPEED
);
// Motor B backward
digitalWrite
(
IN3
,
LOW
);
digitalWrite
(
IN4
,
HIGH
);
analogWrite
(
ENB
,
SPEED
);
currentState
=
STATE_BACKWARD
;
digitalWrite
(
MOVING_LED
,
HIGH
);
}
}
void
executeLeft
()
{
if
(
currentState
!=
STATE_LEFT
)
{
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
()
{
if
(
currentState
!=
STATE_RIGHT
)
{
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, or stop"
);
}
\ No newline at end of file
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment