#include<h2>
// ----------------- TB6612FNG Motor Driver -----------------
const int AIN1 = 4;
const int AIN2 = 5;
const int PWMA = 3;
const int BIN1 = 7;
const int BIN2 = 8;
const int PWMB = 6;
const int STBY = 2;
// ----------------- Multiplexer -----------------
const int sigPin = A0;
const int sPins[4] = {9, 10, 11, 12}; // S0..S3
const int totalChannels = 8; // HY-S301 has 8 sensors
// ----------------- PID Variables -----------------
float Kp = 25; // proportional gain
float Ki = 0; // integral gain
float Kd = 15; // derivative gain
int baseSpeed = 150;
int maxSpeed = 255;
float error = 0, lastError = 0, integral = 0;
// ----------------- Setup -----------------
void setup() {
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
pinMode(STBY, OUTPUT);
for (int i = 0; i < 4; i++) pinMode(sPins[i], OUTPUT);
digitalWrite(STBY, HIGH); // Enable TB6612FNG
[Link](115200);
}
// ----------------- Multiplexer Read -----------------
int readMux(int channel) {
for (int b = 0; b < 4; b++) {
digitalWrite(sPins[b], (channel >> b) & 1);
delayMicroseconds(50);
return analogRead(sigPin);
// ----------------- Sensor Reading -----------------
int readLine() {
long sum = 0;
int activeCount = 0;
for (int i = 0; i < totalChannels; i++) {
int val = readMux(i);
int bin = (val < 500) ? 1 : 0; // threshold, adjust if needed
if (bin == 1) {
sum += i * 1000; // weighted position
activeCount++;
}
if (activeCount == 0) {
return -1; // no line detected
return sum / activeCount; // average position
// ----------------- Motor Control -----------------
void setMotor(int inA1, int inA2, int inB1, int inB2, int speed
A, int speedB) {
digitalWrite(AIN1, inA1);
digitalWrite(AIN2, inA2);
analogWrite(PWMA, constrain(speedA, 0, maxSpeed));
digitalWrite(BIN1, inB1);
digitalWrite(BIN2, inB2);
analogWrite(PWMB, constrain(speedB, 0, maxSpeed));
}
// ----------------- Main Loop -----------------
void loop() {
int position = readLine();
if (position == -1) {
// Lost line → stop
setMotor(LOW, LOW, LOW, LOW, 0, 0);
return;
// Error relative to center (center = 3500 for 8 sensors)
error = position - 3500;
// PID
integral += error;
float derivative = error - lastError;
float output = Kp * error + Ki * integral + Kd * derivative;
lastError = error;
// Adjust motor speeds
int leftSpeed = baseSpeed + output;
int rightSpeed = baseSpeed - output;
// Forward motion
if (leftSpeed >= 0 && rightSpeed >= 0) {
setMotor(HIGH, LOW, HIGH, LOW, leftSpeed, rightSpeed);
// Left motor reverse, right motor forward
else if (leftSpeed < 0 && rightSpeed >= 0) {
setMotor(LOW, HIGH, HIGH, LOW, -leftSpeed, rightSpeed);
// Right motor reverse, left motor forward
else if (rightSpeed < 0 && leftSpeed >= 0) {
setMotor(HIGH, LOW, LOW, HIGH, leftSpeed, -rightSpeed);
// Both reverse
else {
setMotor(LOW, HIGH, LOW, HIGH, -leftSpeed, -right-
Speed);
}
delay(10);