// --- Includes and Setup from Keyestudio Examples ---
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// Initialize Adafruit PWM Servo Driver (address 0x47 is common)
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x47);
// Define sensor pins (as per Keyestudio manual)
#define SENSOR_LEFT_PIN 6
#define SENSOR_MIDDLE_PIN 7
#define SENSOR_RIGHT_PIN 8
// Define motor speeds (0-4095, but Adafruit library takes 0-4095 for on/off, speed
is tricky)
// For simplicity, I will use a base speed and adjust. The manual uses ~1000-2000
for speed.
int baseSpeed = 1500; // Adjust as needed
int turnSpeed = 1200; // Slower speed for turns
// Sensor states
bool sensorL, sensorM, sensorR;
// --- Path Navigation State Machine ---
enum PathState {
MOVING_UP_COL0,
TURNING_RIGHT_AT_COL0_END,
MOVING_RIGHT_TO_COL1,
TURNING_RIGHT_AT_COL1_START,
MOVING_DOWN_COL1,
TURNING_RIGHT_AT_COL1_END,
MOVING_RIGHT_TO_COL2,
TURNING_LEFT_AT_COL2_START,
MOVING_UP_COL2,
TURNING_RIGHT_AT_COL2_END,
MOVING_RIGHT_TO_COL3,
TURNING_RIGHT_AT_COL3_START,
MOVING_DOWN_COL3,
TURNING_RIGHT_AT_COL3_END,
MOVING_RIGHT_TO_COL4,
TURNING_LEFT_AT_COL4_START,
MOVING_UP_COL4,
PATH_COMPLETE
};
PathState currentPathState = MOVING_UP_COL0;
int intersectionsCrossed = 0;
int targetIntersections = 0; // Will be set based on state
bool atJunctionFlag = false; // To handle junction once
// --- Low-Level Motor Control Functions (from Keyestudio examples) ---
// These functions would set the 8 PWM channels for 4 motors
// MA: channels 6 (DIR), 7 (PWM)
// MA1: channels 4 (DIR), 5 (PWM)
// MB: channels 0 (DIR), 1 (PWM)
// MB1: channels 2 (DIR), 3 (PWM)
// DIR: 0,0,0 = stop/coast, 0,0,4095 = one way, 4,0,0 = other way (example, might
be reversed)
// PWM: 0,0,speed_val
void motorSet(int motorNum, bool forward, int speedVal) {
// motorNum: 0=MB, 1=MB1, 2=MA1, 3=MA
// This is a simplified abstraction. Refer to your wiring and the example code.
// Example for MB (motor 0):
// pwm.setPWM(dir_pin, 0, forward ? 4095 : 0); // Or 0 for forward, 4095 for
reverse
// pwm.setPWM(pwm_pin, 0, speedVal);
// Based on Project 8 (Motor Driving) from your manual:
// pwm.setPWM(0,0,4095) & pwm.setPWM(1,0,speed) -> MB clockwise
// pwm.setPWM(0,0,0) & pwm.setPWM(1,0,speed) -> MB anti-clockwise
// Similar for MB1(2,3), MA1(4,5), MA(6,7)
// Let's define pins for clarity (these are PWM channels, not Arduino pins)
int MB_DIR = 0, MB_PWM = 1;
int MB1_DIR = 2, MB1_PWM = 3;
int MA1_DIR = 4, MA1_PWM = 5;
int MA_DIR = 6, MA_PWM = 7;
// Assuming MA/MA1 are right side, MB/MB1 are left side from driver's
perspective
// And forward means positive speed input
// This needs careful calibration based on your physical motor wiring and
shield behavior!
if (motorNum == 0) { // MB (Left Front)
pwm.setPWM(MB_DIR, 0, forward ? 4095 : 0); pwm.setPWM(MB_PWM, 0, speedVal);
} else if (motorNum == 1) { // MB1 (Left Rear)
pwm.setPWM(MB1_DIR, 0, forward ? 4095 : 0); pwm.setPWM(MB1_PWM, 0,
speedVal);
} else if (motorNum == 2) { // MA1 (Right Rear)
pwm.setPWM(MA1_DIR, 0, forward ? 4095 : 0); pwm.setPWM(MA1_PWM, 0,
speedVal);
} else if (motorNum == 3) { // MA (Right Front)
pwm.setPWM(MA_DIR, 0, forward ? 4095 : 0); pwm.setPWM(MA_PWM, 0, speedVal);
}
}
void goForward(int speed) {
// Assuming MA/MA1 are right motors, MB/MB1 are left motors
// And all need to spin "forward"
motorSet(0, true, speed); // MB (Left Front)
motorSet(1, true, speed); // MB1 (Left Rear)
motorSet(2, true, speed); // MA1 (Right Rear)
motorSet(3, true, speed); // MA (Right Front)
}
void goBackward(int speed) {
motorSet(0, false, speed);
motorSet(1, false, speed);
motorSet(2, false, speed);
motorSet(3, false, speed);
}
void turnLeft(int speed) { // Pivot turn: Left motors back, Right motors forward
motorSet(0, false, speed); // MB (Left Front) backward
motorSet(1, false, speed); // MB1 (Left Rear) backward
motorSet(2, true, speed); // MA1 (Right Rear) forward
motorSet(3, true, speed); // MA (Right Front) forward
}
void turnRight(int speed) { // Pivot turn: Left motors forward, Right motors back
motorSet(0, true, speed); // MB (Left Front) forward
motorSet(1, true, speed); // MB1 (Left Rear) forward
motorSet(2, false, speed); // MA1 (Right Rear) backward
motorSet(3, false, speed); // MA (Right Front) backward
}
void stopMotors() {
motorSet(0, true, 0);
motorSet(1, true, 0);
motorSet(2, true, 0);
motorSet(3, true, 0);
}
// --- Higher-Level Movement Abstractions ---
void moveForwardSlightly() {
goForward(baseSpeed / 2); // Slower speed to cross junction
delay(300); // Adjust delay to move just past the center of intersection
// stopMotors(); // Optional: stop after, or let line follower take over
}
void turnRight90() {
// Turn right for a fixed duration/angle
// This needs calibration!
stopMotors();
delay(100);
turnRight(turnSpeed);
delay(500); // Adjust this delay for a ~90 degree turn
stopMotors();
delay(100);
intersectionsCrossed = 0; // Reset for the new segment
atJunctionFlag = false;
}
void turnLeft90() {
// Turn left for a fixed duration/angle
// This needs calibration!
stopMotors();
delay(100);
turnLeft(turnSpeed);
delay(500); // Adjust this delay for a ~90 degree turn
stopMotors();
delay(100);
intersectionsCrossed = 0; // Reset for the new segment
atJunctionFlag = false;
}
// --- Sensor Reading ---
void readLineSensors() {
// Remember: LOW = black line, HIGH = white
sensorL = (digitalRead(SENSOR_LEFT_PIN) == LOW);
sensorM = (digitalRead(SENSOR_MIDDLE_PIN) == LOW);
sensorR = (digitalRead(SENSOR_RIGHT_PIN) == LOW);
}
bool isJunction() {
// Junction detected if all three sensors see black
return sensorL && sensorM && sensorR;
}
// --- Line Following Logic ---
void followLineBasic() {
if (sensorM) { // Middle on line
goForward(baseSpeed);
} else if (sensorL && !sensorR) { // Left on line, Right is not (to avoid
issues at sharp turns)
// Gentle turn right (slow down left motors or speed up right, or
combination)
motorSet(0, true, baseSpeed / 2); // MB (Left Front) slower
motorSet(1, true, baseSpeed / 2); // MB1 (Left Rear) slower
motorSet(2, true, baseSpeed); // MA1 (Right Rear) normal
motorSet(3, true, baseSpeed); // MA (Right Front) normal
} else if (sensorR && !sensorL) { // Right on line, Left is not
// Gentle turn left
motorSet(0, true, baseSpeed); // MB (Left Front) normal
motorSet(1, true, baseSpeed); // MB1 (Left Rear) normal
motorSet(2, true, baseSpeed / 2); // MA1 (Right Rear) slower
motorSet(3, true, baseSpeed / 2); // MA (Right Front) slower
} else if (sensorL && sensorR && !sensorM) { // Both L & R on, M off (unlikely
with 1cm spacing on 2cm line, but for robustness)
goForward(baseSpeed); // Or could be an error state
}
else { // Lost line (no sensors see black)
// Could stop, or try to find line (e.g., last known direction)
// For now, let's assume it mostly stays on track or re-centers quickly
// Or if !sensorL && !sensorM && !sensorR
stopMotors(); // Stop if completely lost
}
}
void setTargetIntersectionsForState() {
switch (currentPathState) {
case MOVING_UP_COL0: targetIntersections = 2; break;
case MOVING_RIGHT_TO_COL1:targetIntersections = 1; break;
case MOVING_DOWN_COL1: targetIntersections = 4; break;
case MOVING_RIGHT_TO_COL2:targetIntersections = 1; break;
case MOVING_UP_COL2: targetIntersections = 4; break;
case MOVING_RIGHT_TO_COL3:targetIntersections = 1; break;
case MOVING_DOWN_COL3: targetIntersections = 4; break;
case MOVING_RIGHT_TO_COL4:targetIntersections = 1; break;
case MOVING_UP_COL4: targetIntersections = 4; break; // Or to center
then stop
default: targetIntersections = 0; break; // Should not
happen in active move states
}
}
// --- Main Setup and Loop ---
void setup() {
Serial.begin(9600);
pwm.begin();
pwm.setPWMFreq(60); // Standard servo/motor frequency
pinMode(SENSOR_LEFT_PIN, INPUT);
pinMode(SENSOR_MIDDLE_PIN, INPUT);
pinMode(SENSOR_RIGHT_PIN, INPUT);
Serial.println("Robot Starting Path Navigation...");
stopMotors();
delay(1000);
setTargetIntersectionsForState(); // Set initial target
}
void loop() {
if (currentPathState == PATH_COMPLETE) {
Serial.println("Path Complete!");
stopMotors();
while(true); // Halt
}
readLineSensors();
if (isJunction()) {
if (!atJunctionFlag) { // Process this junction only once
atJunctionFlag = true; // Set flag to prevent re-entry for this same
physical junction
Serial.print("Junction detected. State: ");
Serial.print(currentPathState);
Serial.print(", Intersections: ");
Serial.println(intersectionsCrossed);
stopMotors();
delay(100); // Short pause
moveForwardSlightly(); // Move forward to center over/past the junction
line
intersectionsCrossed++;
Serial.print("Crossed intersection. Total now: ");
Serial.println(intersectionsCrossed);
if (intersectionsCrossed >= targetIntersections) {
Serial.println("Target intersections reached for this segment.");
// Reached end of current straight segment, decide next state/turn
switch (currentPathState) {
case MOVING_UP_COL0:
currentPathState = TURNING_RIGHT_AT_COL0_END;
Serial.println("Transitioning to
TURNING_RIGHT_AT_COL0_END");
break;
case MOVING_RIGHT_TO_COL1:
currentPathState = TURNING_RIGHT_AT_COL1_START;
Serial.println("Transitioning to
TURNING_RIGHT_AT_COL1_START");
break;
case MOVING_DOWN_COL1:
currentPathState = TURNING_RIGHT_AT_COL1_END;
Serial.println("Transitioning to
TURNING_RIGHT_AT_COL1_END");
break;
case MOVING_RIGHT_TO_COL2:
currentPathState = TURNING_LEFT_AT_COL2_START;
Serial.println("Transitioning to
TURNING_LEFT_AT_COL2_START");
break;
case MOVING_UP_COL2:
currentPathState = TURNING_RIGHT_AT_COL2_END;
Serial.println("Transitioning to
TURNING_RIGHT_AT_COL2_END");
break;
case MOVING_RIGHT_TO_COL3:
currentPathState = TURNING_RIGHT_AT_COL3_START;
Serial.println("Transitioning to
TURNING_RIGHT_AT_COL3_START");
break;
case MOVING_DOWN_COL3:
currentPathState = TURNING_RIGHT_AT_COL3_END;
Serial.println("Transitioning to
TURNING_RIGHT_AT_COL3_END");
break;
case MOVING_RIGHT_TO_COL4:
currentPathState = TURNING_LEFT_AT_COL4_START;
Serial.println("Transitioning to
TURNING_LEFT_AT_COL4_START");
break;
case MOVING_UP_COL4:
currentPathState = PATH_COMPLETE; // End of path
Serial.println("Transitioning to PATH_COMPLETE");
break;
default: break;
}
} else {
// Intermediate junction, continue straight
Serial.println("Intermediate junction, continuing.");
// atJunctionFlag will be reset when line following resumes and no
longer sees a junction
}
}
// If still atJunctionFlag is true, it means I am still on the junction,
// I might need to ensure followLineBasic() or a state transition occurs.
// The flag is mainly to correctly count intersections.
// Let's add a small delay to ensure sensors clear the junction before
resetting the flag.
// The flag will naturally reset when `isJunction()` becomes false.
} else { // Not at a junction
atJunctionFlag = false; // Reset flag if I are off the junction
// Handle turns or continue line following
switch (currentPathState) {
case TURNING_RIGHT_AT_COL0_END:
turnRight90();
currentPathState = MOVING_RIGHT_TO_COL1;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_RIGHT_TO_COL1");
break;
case TURNING_RIGHT_AT_COL1_START:
turnRight90();
currentPathState = MOVING_DOWN_COL1;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_DOWN_COL1");
break;
case TURNING_RIGHT_AT_COL1_END:
turnRight90();
currentPathState = MOVING_RIGHT_TO_COL2;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_RIGHT_TO_COL2");
break;
case TURNING_LEFT_AT_COL2_START:
turnLeft90();
currentPathState = MOVING_UP_COL2;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_UP_COL2");
break;
case TURNING_RIGHT_AT_COL2_END:
turnRight90();
currentPathState = MOVING_RIGHT_TO_COL3;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_RIGHT_TO_COL3");
break;
case TURNING_RIGHT_AT_COL3_START:
turnRight90();
currentPathState = MOVING_DOWN_COL3;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_DOWN_COL3");
break;
case TURNING_RIGHT_AT_COL3_END:
turnRight90();
currentPathState = MOVING_RIGHT_TO_COL4;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_RIGHT_TO_COL4");
break;
case TURNING_LEFT_AT_COL4_START:
turnLeft90();
currentPathState = MOVING_UP_COL4;
setTargetIntersectionsForState();
Serial.println("Turned. New state: MOVING_UP_COL4");
break;
// If in a moving state, just follow the line
case MOVING_UP_COL0:
case MOVING_RIGHT_TO_COL1:
case MOVING_DOWN_COL1:
case MOVING_RIGHT_TO_COL2:
case MOVING_UP_COL2:
case MOVING_RIGHT_TO_COL3:
case MOVING_DOWN_COL3:
case MOVING_RIGHT_TO_COL4:
case MOVING_UP_COL4:
followLineBasic();
break;
default: // Includes PATH_COMPLETE, do nothing
break;
}
}
delay(10); // Small delay for stability
}
