J'ai apporté des modifications de « sortie » à votre programme d'origine. Il ne s'imprime que lorsqu'il y a de nouvelles informations. Pour trouver toutes mes entrées, cherchez « OLD »
#include <math.h> // Include math library for exponential and logarithmic functions
// ---------------------------
// Pin Definitions
// ---------------------------
const int photoresistorPinDx = A1; // Right photoresistor connected to analog pin A1
const int photoresistorPinSx = A0; // Left photoresistor connected to analog pin A0
const int trigPin = 7; // Trig pin connected to digital pin 7
const int echoPin = 6; // Echo pin connected to digital pin 6
// Motor control pins connected to IN1-IN4 of the MX1508 motor driver
const int motorIn1 = 9;
const int motorIn2 = 10;
const int motorIn3 = 3;
const int motorIn4 = 5;
const int switchPin = A5; // Switch connected to analog pin A5
const int statusLedPin = 13; // Status LED connected to digital pin 13
// ---------------------------
// Behavior Constants
// ---------------------------
const int LOVE = 1;
const int FEAR = 2;
const int AGGRESSION = 3;
const int ALIVENESS = 4;
// Behavior Variable
int behavior = 0; // Initially no behavior selected
// ---------------------------
// State Machine
// ---------------------------
enum State { OFF, STANDBY, RUNNING };
State currentState = STANDBY; // Start in STANDBY mode
// ---------------------------
// Constants and Variables
// ---------------------------
const int distanceThreshold = 10; // Threshold distance in cm to stop the motors
int rawSensorValueDx = 0; // Raw value from the right photoresistor (0-1023)
int normalizedValueDx = 0; // Normalized right photoresistor value
int rawSensorValueSx = 0; // Raw value from the left photoresistor (0-1023)
int normalizedValueSx = 0; // Normalized left photoresistor value
long duration = 0; // Time taken by the ultrasonic pulse (microseconds)
int distance = 0; // Calculated distance from the sensor (cm)
int motorSpeedDx = 0; // Right motor speed (-255 to 255)
int motorSpeedSx = 0; // Left motor speed (-255 to 255)
float voltage = 0.0; // Voltage at voltagePin
// float current = 0.0; // Current flowing through the motor (Removed)
// Button Timing Variables
unsigned long buttonPressStartTime = 0;
bool buttonPreviouslyPressed = false;
unsigned long lastButtonPressTime = 0;
int buttonPressCount = 0;
const unsigned long buttonPressTimeWindow = 1500; // 1.5 seconds time window for counting presses
const unsigned long shortPressThreshold = 1000; // 1 second for short press
const unsigned long longPressThreshold = 3000; // 3 seconds for long press
// Calibration Constants
const int normalizationOffset = 300; // Adjust based on calibration
const int maxAverageLight = 600; // Adjust based on calibration
const float scalingFactor = 1.2; // 20% speed increase
const int minSpeed = 100; // Minimum motor speed
// Turn Constants
const int turnSpeed = 150; // Speed for turning (Adjust as needed)
const unsigned long turnDuration = 500; // Duration to turn 90 degrees in milliseconds (Adjust based on testing)
// Direction Inversion Flags
const bool invertMotorA = false; // Set to true to invert Motor A direction
const bool invertMotorB = false; // Set to true to invert Motor B direction
// ---------------------------
// Function Prototypes
// ---------------------------
void stopMotors();
void setMotorA(int speed);
void setMotorB(int speed);
void blinkLed();
void turnLeft90(); // New function for turning left 90 degrees
// ---------------------------
// Setup Function
// ---------------------------
void setup()
{
// Initialize Serial Communication at 9600 baud rate
Serial.begin(9600);
Serial.println("Motor Control Initialized");
// Initialize motor pins as OUTPUT
pinMode(motorIn1, OUTPUT);
pinMode(motorIn2, OUTPUT);
pinMode(motorIn3, OUTPUT);
pinMode(motorIn4, OUTPUT);
// Initialize ultrasonic sensor pins
pinMode(trigPin, OUTPUT); // Trig pin as OUTPUT
pinMode(echoPin, INPUT); // Echo pin as INPUT
// Initialize switch and LED pins
pinMode(switchPin, INPUT_PULLUP);
pinMode(statusLedPin, OUTPUT);
// Initialize state to STANDBY
currentState = STANDBY;
Serial.println("System started in STANDBY mode.");
// Set initial motor pins to LOW
digitalWrite(motorIn1, LOW);
digitalWrite(motorIn2, LOW);
digitalWrite(motorIn3, LOW);
digitalWrite(motorIn4, LOW);
}
// ---------------------------
// Function to Stop Motors
// ---------------------------
void stopMotors() {
digitalWrite(motorIn1, LOW);
digitalWrite(motorIn2, LOW);
digitalWrite(motorIn3, LOW);
digitalWrite(motorIn4, LOW);
}
// ---------------------------
// Functions to Control Motors
// ---------------------------
void setMotorA(int speed) {
// Invert speed if inversion flag is true
if (invertMotorA) {
speed = -speed;
}
if (speed > 0) {
// Forward
analogWrite(motorIn1, speed); // PWM speed control
digitalWrite(motorIn2, LOW);
} else if (speed < 0) {
// Reverse
digitalWrite(motorIn1, LOW);
analogWrite(motorIn2, -speed); // PWM speed control
} else {
// Stop motor
digitalWrite(motorIn1, LOW);
digitalWrite(motorIn2, LOW);
}
}
void setMotorB(int speed) {
// Invert speed if inversion flag is true
if (invertMotorB) {
speed = -speed;
}
if (speed > 0) {
// Forward
analogWrite(motorIn3, speed); // PWM speed control
digitalWrite(motorIn4, LOW);
} else if (speed < 0) {
// Reverse
digitalWrite(motorIn3, LOW);
analogWrite(motorIn4, -speed); // PWM speed control
} else {
// Stop motor
digitalWrite(motorIn3, LOW);
digitalWrite(motorIn4, LOW);
}
}
// ---------------------------
// Blink LED Function
// ---------------------------
void blinkLed()
{
static unsigned long previousMillis = 0;
static bool ledState = false;
const unsigned long interval = 500; // Blink interval in ms
unsigned long currentMillis = millis();
if (currentMillis - previousMillis >= interval)
{
previousMillis = currentMillis;
ledState = !ledState;
digitalWrite(statusLedPin, ledState ? HIGH : LOW);
}
}
// ---------------------------
// Turn Left 90 Degrees Function
// ---------------------------
void turnLeft90() {
Serial.println("Initiating 90-degree left turn.");
// Set motors to turn left
setMotorA(-turnSpeed); // Left motor backward
setMotorB(turnSpeed); // Right motor forward
delay(turnDuration); // Duration to turn 90 degrees (calibrate as needed)
stopMotors(); // Stop motors after turning
Serial.println("90-degree left turn completed.");
}
// ---------------------------
// Loop Function
// ---------------------------
int OLDbuttonState,
OLDcurrentState,
OLDrawSensorValueSx,
OLDrawSensorValueDx,
OLDnormalizedValueDx,
OLDdistance,
OLDmotorSpeedDx,
OLDmotorSpeedSx;
void loop()
{
// Read the switch state
int buttonState = digitalRead(switchPin);
if (OLDcurrentState != currentState) {
OLDcurrentState = currentState;
// Print the current state
Serial.print("Current State: ");
if (currentState == OFF) Serial.println("OFF");
else if (currentState == STANDBY) Serial.println("STANDBY");
else if (currentState == RUNNING) Serial.println("RUNNING");
}
if (OLDbuttonState != buttonState) {
OLDbuttonState = buttonState;
// Print the switch state
Serial.print("Switch State: ");
if (buttonState == LOW) Serial.println("PRESSED");
else Serial.println("RELEASED");
}
if (currentState == STANDBY)
{
// Blink the LED connected to statusLedPin
blinkLed();
// Button handling in STANDBY mode
if (buttonState == LOW) // Assuming LOW means pressed
{
if (!buttonPreviouslyPressed)
{
// Button was just pressed
buttonPressStartTime = millis();
buttonPreviouslyPressed = true;
Serial.println("Button Pressed.");
}
else
{
// Button is being held
unsigned long buttonHoldTime = millis() - buttonPressStartTime;
if (buttonHoldTime >= longPressThreshold)
{
// Long press detected, switch to OFF state
currentState = OFF;
buttonPreviouslyPressed = false;
Serial.println("Long Press Detected. Switching to OFF state.");
// Turn off LED
digitalWrite(statusLedPin, LOW);
// Reset variables
buttonPressCount = 0;
}
}
}
else
{
if (buttonPreviouslyPressed)
{
// Button was just released
unsigned long buttonPressDuration = millis() - buttonPressStartTime;
if (buttonPressDuration < shortPressThreshold)
{
// Short press detected
buttonPressCount++;
lastButtonPressTime = millis();
Serial.print("Short Press Detected. Count: ");
Serial.println(buttonPressCount);
}
// Reset
buttonPreviouslyPressed = false;
Serial.println("Button Released.");
}
}
// Process button presses for behavior selection
if (buttonPressCount > 0 && (millis() - lastButtonPressTime > buttonPressTimeWindow))
{
// Time window expired, select behavior
if (buttonPressCount == 1)
{
behavior = LOVE;
Serial.println("Behavior Selected: LOVE");
}
else if (buttonPressCount == 2)
{
behavior = FEAR;
Serial.println("Behavior Selected: FEAR");
}
else if (buttonPressCount == 3)
{
behavior = AGGRESSION;
Serial.println("Behavior Selected: AGGRESSION");
}
else if (buttonPressCount == 4)
{
behavior = ALIVENESS;
Serial.println("Behavior Selected: ALIVENESS");
}
else
{
// Invalid number of presses, do nothing
behavior = 0;
Serial.println("Invalid number of button presses.");
}
if (behavior != 0)
{
currentState = RUNNING;
// Turn on the LED
digitalWrite(statusLedPin, HIGH);
Serial.println("Switching to RUNNING state.");
}
// Reset buttonPressCount
buttonPressCount = 0;
}
// In STANDBY, no motors should be running
stopMotors();
}
else if (currentState == RUNNING)
{
// Check if the button is pressed to go back to STANDBY
if (buttonState == LOW && !buttonPreviouslyPressed)
{
// Button pressed, go back to STANDBY
currentState = STANDBY;
// Turn off the LED
digitalWrite(statusLedPin, LOW);
// Reset behavior
behavior = 0;
// Reset button variables
buttonPreviouslyPressed = true;
Serial.println("Button Pressed. Returning to STANDBY Mode.");
// Stop motors
stopMotors();
}
else if (buttonState == HIGH && buttonPreviouslyPressed)
{
// Button released
buttonPreviouslyPressed = false;
Serial.println("Button Released.");
}
// Perform the selected behavior
// ---------------------------
// Ultrasonic Sensor Reading
// ---------------------------
// Clear the trigPin by setting it LOW for 2 microseconds
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
// Trigger the sensor by setting the trigPin HIGH for 10 microseconds
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the echoPin, returns the sound wave travel time in microseconds
duration = pulseIn(echoPin, HIGH, 30000); // Timeout after 30ms to prevent blocking
// ---------------------------
// Inserted Debugging Code
// ---------------------------
if (duration == 0) {
Serial.println("Timeout: No echo detected.");
}
// ---------------------------
// Calculate the distance (in cm)
distance = duration * 0.0343 / 2; // Speed of sound wave divided by 2 (go and back)
if (OLDdistance != distance) {
OLDdistance = distance;
// Print ultrasonic sensor reading
Serial.print("Ultrasonic Distance: ");
Serial.print(distance);
Serial.println(" cm");
}
// ---------------------------
// Obstacle Detection and Motor Control
// ---------------------------
if (distance > 0 && distance < distanceThreshold)
{
// Obstacle detected within threshold distance, stop the motors
motorSpeedDx = 0;
motorSpeedSx = 0;
stopMotors();
if (OLDdistance != distance) {
// Print status to Serial Monitor
Serial.print("Obstacle detected! Distance: ");
Serial.print(distance);
Serial.println(" cm. Motors stopped.");
}
// Perform a 90-degree left turn
turnLeft90();
}
else
{
// No obstacle, proceed with normal operation
// ---------------------------
// Photoresistor Reading and Normalization
// ---------------------------
rawSensorValueDx = analogRead(photoresistorPinDx); // Read raw value from right photoresistor
rawSensorValueSx = analogRead(photoresistorPinSx); // Read raw value from left photoresistor
// Normalize the sensor values by subtracting the offset
normalizedValueDx = rawSensorValueDx - normalizationOffset;
normalizedValueSx = rawSensorValueSx - normalizationOffset;
// Ensure the normalized values are not negative
normalizedValueDx = max(normalizedValueDx, 0);
normalizedValueSx = max(normalizedValueSx, 0);
if (OLDrawSensorValueDx != rawSensorValueDx || OLDnormalizedValueDx != normalizedValueDx || OLDrawSensorValueSx != rawSensorValueSx) {
OLDrawSensorValueDx = rawSensorValueDx;
OLDnormalizedValueDx = normalizedValueDx;
OLDrawSensorValueSx = rawSensorValueSx;
// Print photoresistor readings
Serial.print("Photoresistor Dx Raw: ");
Serial.print(rawSensorValueDx);
Serial.print(", Normalized: ");
Serial.print(normalizedValueDx);
Serial.print(" | Photoresistor Sx Raw: ");
Serial.print(rawSensorValueSx);
Serial.print(", Normalized: ");
Serial.println(normalizedValueSx);
}
// ---------------------------
// Behavior Logic
// ---------------------------
int averageLight = (normalizedValueDx + normalizedValueSx) / 2;
int error = normalizedValueDx - normalizedValueSx; // Difference between right and left sensor readings
int baseSpeed = 0;
int adjustment = 0;
// Constrain averageLight to prevent overflow in map()
averageLight = constrain(averageLight, 0, maxAverageLight);
if (behavior == LOVE)
{
// LOVE behavior: Motors speed decreases as light intensity increases
baseSpeed = 255 - map(averageLight, 0, maxAverageLight, 0, 255);
baseSpeed = baseSpeed * scalingFactor;
baseSpeed = constrain(baseSpeed, minSpeed, 255); // Apply minimum speed
adjustment = (error * baseSpeed) / (2 * maxAverageLight); // Proportional adjustment based on error
motorSpeedDx = baseSpeed - adjustment;
motorSpeedSx = baseSpeed + adjustment;
}
else if (behavior == FEAR)
{
// FEAR behavior: Motors speed increases as light intensity increases
baseSpeed = map(averageLight, 0, maxAverageLight, 0, 255);
baseSpeed = baseSpeed * scalingFactor;
baseSpeed = constrain(baseSpeed, minSpeed, 255); // Apply minimum speed
adjustment = (error * baseSpeed) / (2 * maxAverageLight);
motorSpeedDx = baseSpeed + adjustment;
motorSpeedSx = baseSpeed - adjustment;
}
else if (behavior == AGGRESSION)
{
// AGGRESSION behavior: Motors speed inversely related to light intensity
baseSpeed = map(averageLight, 0, maxAverageLight, 255, 0);
baseSpeed = baseSpeed * scalingFactor;
baseSpeed = constrain(baseSpeed, minSpeed, 255); // Apply minimum speed
adjustment = (error * baseSpeed) / (2 * maxAverageLight);
motorSpeedDx = baseSpeed - adjustment;
motorSpeedSx = baseSpeed + adjustment;
}
else if (behavior == ALIVENESS)
{
// ALIVENESS behavior: Step function with three intervals
if (averageLight <= (maxAverageLight / 3)) // 1/3 of maxAverageLight
{
// First third: exponential increase
float x = (float)averageLight / (maxAverageLight / 3.0); // Normalize to 0-1
float y = (exp(3 * x) - 1) / (exp(3) - 1); // Exponential curve
baseSpeed = (int)(y * 255);
}
else if (averageLight <= (2 * maxAverageLight / 3)) // 2/3 of maxAverageLight
{
// Middle third: maximum speed
baseSpeed = 255;
}
else
{
// Last third: decreasing logarithmic function
float x = (float)(averageLight - (2 * maxAverageLight / 3)) / (maxAverageLight / 3.0); // Normalize to 0-1
float y = 1 - (log(9 * x + 1) / log(10)); // Logarithmic decrease
baseSpeed = (int)(y * 255);
}
baseSpeed = baseSpeed * scalingFactor;
baseSpeed = constrain(baseSpeed, minSpeed, 255); // Apply minimum speed
adjustment = (error * baseSpeed) / (2 * maxAverageLight);
motorSpeedDx = baseSpeed - adjustment;
motorSpeedSx = baseSpeed + adjustment;
}
else
{
// Default behavior: Stop the motors if no behavior is selected
motorSpeedDx = 0;
motorSpeedSx = 0;
}
// ---------------------------
// Constrain Motor Speeds
// ---------------------------
motorSpeedDx = constrain(motorSpeedDx, -255, 255); // Ensure motor speed is within -255 to 255
motorSpeedSx = constrain(motorSpeedSx, -255, 255);
// ---------------------------
// Set Motor Speeds
// ---------------------------
setMotorB(motorSpeedDx); // Right motor
setMotorA(motorSpeedSx); // Left motor
// Print motor speeds
if (OLDmotorSpeedDx != motorSpeedDx || OLDmotorSpeedSx != motorSpeedSx) {
OLDmotorSpeedDx = motorSpeedDx;
OLDmotorSpeedSx = motorSpeedSx;
Serial.print("Motor Speeds - Dx: ");
Serial.print(motorSpeedDx);
Serial.print(", Sx: ");
Serial.println(motorSpeedSx);
}
// ---------------------------
// Voltage Measurement (Commented Out)
// ---------------------------
//int voltageRaw = analogRead(voltagePin); // Read raw voltage value from voltage divider
//voltage = (voltageRaw * (5.0 / 1023.0)) * 2; // Multiply by 2 because of voltage divider
// Print voltage
//Serial.print("Voltage: ");
//Serial.print(voltage);
//Serial.println(" V");
}
// Delay at the end of RUNNING state loop
delay(100); // Adjust as necessary
}
}
diagram.json in wokwi.com
{
"version": 1,
"author": "Anonymous maker",
"editor": "wokwi",
"parts": [
{ "type": "wokwi-arduino-nano", "id": "nano", "top": 4.8, "left": -0.5, "attrs": {} },
{ "type": "wokwi-potentiometer", "id": "pot1", "top": -1.3, "left": 201.4, "attrs": {} },
{ "type": "wokwi-potentiometer", "id": "pot2", "top": -1.3, "left": 278.2, "attrs": {} },
{ "type": "wokwi-hc-sr04", "id": "ultrasonic1", "top": -142.5, "left": 216.7, "attrs": {} },
{
"type": "wokwi-led",
"id": "led1",
"top": -145.2,
"left": 169.8,
"rotate": 90,
"attrs": { "color": "green" }
},
{
"type": "wokwi-led",
"id": "led2",
"top": -126,
"left": 169.8,
"rotate": 90,
"attrs": { "color": "white" }
},
{
"type": "wokwi-led",
"id": "led3",
"top": -106.8,
"left": 169.8,
"rotate": 90,
"attrs": { "color": "red" }
},
{
"type": "wokwi-led",
"id": "led4",
"top": -106.8,
"left": -46.6,
"rotate": 270,
"attrs": { "color": "red", "flip": "1" }
},
{
"type": "wokwi-led",
"id": "led5",
"top": -126,
"left": -46.6,
"rotate": 270,
"attrs": { "color": "white", "flip": "1" }
},
{
"type": "wokwi-led",
"id": "led6",
"top": -145.2,
"left": -46.6,
"rotate": 270,
"attrs": { "color": "green", "flip": "1" }
},
{
"type": "wokwi-pushbutton",
"id": "btn1",
"top": 111.8,
"left": 19.2,
"attrs": { "color": "green" }
}
],
"connections": [
[ "pot1:GND", "nano:GND.1", "black", [ "v19.2", "h-76.8" ] ],
[ "pot1:SIG", "nano:A0", "green", [ "v28.8", "h-144.4" ] ],
[ "pot2:SIG", "nano:A1", "green", [ "v38.4", "h-240.4" ] ],
[ "pot1:VCC", "nano:5V", "red", [ "v48", "h-96.8" ] ],
[ "pot2:GND", "nano:GND.1", "black", [ "v19.2", "h-163.2" ] ],
[ "pot2:VCC", "nano:5V", "red", [ "v48", "h-192.8" ] ],
[ "ultrasonic1:TRIG", "nano:7", "green", [ "v19.2", "h-154" ] ],
[ "ultrasonic1:ECHO", "nano:6", "green", [ "v28.8", "h-192.8" ] ],
[ "ultrasonic1:VCC", "nano:5V", "red", [ "v9.6", "h-134.4" ] ],
[ "ultrasonic1:GND", "nano:GND.1", "black", [ "v38.4", "h-154.8" ] ],
[ "nano:GND.1", "led3:C", "black", [ "v0" ] ],
[ "nano:GND.1", "led2:C", "black", [ "v0" ] ],
[ "nano:GND.1", "led1:C", "black", [ "v0" ] ],
[ "nano:3", "led3:A", "green", [ "v0" ] ],
[ "nano:5", "led1:A", "green", [ "v0" ] ],
[ "nano:GND.1", "led4:C", "black", [ "v0" ] ],
[ "nano:GND.1", "led5:C", "black", [ "v0" ] ],
[ "nano:GND.1", "led6:C", "black", [ "v0" ] ],
[ "nano:10", "led4:A", "green", [ "v0" ] ],
[ "nano:11", "led6:A", "green", [ "v0" ] ],
[ "nano:GND.1", "btn1:1.r", "black", [ "v0" ] ],
[ "nano:A5", "btn1:2.r", "green", [ "v0" ] ],
[ "nano:13", "led5:A", "green", [ "v9.6", "h-19.2", "v-86.4", "h57.6", "v-86.4" ] ],
[ "led5:A", "led2:A", "green", [ "h0" ] ]
],
"dependencies": {}
}