Hello there, I have a deadline in two days please help me!!!
I'm tying to run 2 dc motors using the MX1508 (int1 to 9, int2 to 10, int3 to 3, int4 to 5) based on the values read by two photoresistors connected to pins A0 (left) , A1 (right), and a distance sensor (trig 7, echo 6) connecting everything to a 7.4 lithium battery, but I'm experiencing several issues.
First of all only the right motor (motor A) works, secondly, when I plug it out of the pc the depth sensor stops working and the photoresistors give weird values.
Here is the code, schematics and pictures of the circuit
#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
// ---------------------------
void loop()
{
// Read the switch state
int buttonState = digitalRead(switchPin);
// 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");
// 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)
// 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();
// 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);
// 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
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
}
}
