Scribd
Upload
3 views11 pages

MR Thei

This document contains Arduino code for controlling a robotic system with various sensors and motors. It includes setup for an LCD display, motor control, line following logic, and PID control for precise movement. The code allows the robot to respond to commands and navigate to predefined positions using line sensors and an ultrasonic sensor.

Uploaded by

vttmd2023
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
3 views11 pages

MR Thei

This document contains Arduino code for controlling a robotic system with various sensors and motors. It includes setup for an LCD display, motor control, line following logic, and PID control for precise movement. The code allows the robot to respond to commands and navigate to predefined positions using line sensors and an ultrasonic sensor.

Uploaded by

vttmd2023
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 11

#include <Wire.

h>
#include <LiquidCrystal_I2C.h>
// Khai báo LCD với địa chỉ I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);
// Motor Pins
const int motorA1 = 6;
const int motorA2 = 5;
const int motorAspeed = 4;
const int motorB1 = 8;
const int motorB2 = 9;
const int motorBspeed = 7;
// Line Sensor Pins
const int L_L_S = 40;
const int L_S = 42;
const int S_S = 44;
const int R_S = 46;
const int R_R_S = 48;
// Additional Sensor Pins
const int sensorPin1 = 49;
const int sensorPin2 = 47;
const int sensorPin3 = 45;
const int sensorPin4 = 43;
const int sensorPin5 = 51;
// Ultrasonic Sensor Pins
const int TRIG_PIN = 3;
const int ECHO_PIN = 2;
// Khai báo các chân
const int IN1 = 31;
const int IN2 = 33;
const int LIMIT_SWITCH1 = 35;
const int IN3 = 30;
const int IN4 = 32;
const int LIMIT_SWITCH2 = 34;
const int INFRARED_SENSOR = 37;

const int LIMIT_SWITCH_TOP = 39;


const int LED_PIN = 53;

unsigned long timeoutMillis = 10000;


bool motor1Running = false;
bool motor2Running = false;

long duration;
int distance;

unsigned long lastValidSensorTime = 0;


int stop_distance = 10;
// PID Variables
float Kp = 130; // Hệ số P
float Ki = 0.2; // Hệ số I
float Kd = 10; // Hệ số D
int lastError = 0;
int integral = 0;

// Motor Speed Limits


const int maxSpeed = 255;
const int baseSpeed = 240;

void setup() {
pinMode(L_L_S, INPUT);
pinMode(L_S, INPUT);
pinMode(S_S, INPUT);
pinMode(R_S, INPUT);
pinMode(R_R_S, INPUT);

pinMode(motorA1, OUTPUT);
pinMode(motorA2, OUTPUT);
pinMode(motorB1, OUTPUT);
pinMode(motorB2, OUTPUT);
pinMode(motorAspeed, OUTPUT);
pinMode(motorBspeed, OUTPUT);

pinMode(sensorPin5, INPUT_PULLUP);
pinMode(sensorPin1, INPUT);
pinMode(sensorPin2, INPUT);
pinMode(sensorPin3, INPUT);
pinMode(sensorPin4, INPUT);

pinMode(TRIG_PIN, OUTPUT);
pinMode(ECHO_PIN, INPUT);

pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
pinMode(LIMIT_SWITCH1, INPUT_PULLUP);

pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
pinMode(LIMIT_SWITCH2, INPUT_PULLUP);
pinMode(INFRARED_SENSOR, INPUT_PULLUP);

pinMode(LIMIT_SWITCH_TOP, INPUT_PULLUP);
pinMode(LED_PIN, OUTPUT);

Serial.begin(9600);
Serial1.begin(115200);
lcd.begin();
lcd.backlight();
lcd.setCursor(2, 0);
lcd.print("He thong OK!");
delay(1000);

calibrateSensors();
moveMotorsHome();
//reverseMotorsAfterHome();
}
///////////////////////////////////////////////////////////////////////////////////
///
void loop() {
lcd.setCursor(0, 0);
lcd.print("Dang cho lenh...");
blinkLED();

checkLimitSwitch(LIMIT_SWITCH1, IN1, IN2, "Động cơ 1");


checkLimitSwitch(LIMIT_SWITCH2, IN3, IN4, "Động cơ 2");

if (digitalRead(LIMIT_SWITCH_TOP) == LOW) {
stopMotors();
//Serial.println("Động cơ dừng do công tắc hành trình giới hạn trên.");
}

if (Serial1.available()) {
String command = Serial1.readString();
command.trim();

lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Lenh: " + command);

if (command == "11") {
motor1Running = true;
motor2Running = true;
findStartPosition();
moveToPosition1();
reverseMotorsAfterHome();
findStartPosition();
} else if (command == "22") {
motor1Running = true;
motor2Running = true;
findStartPosition();
moveToPosition2();
reverseMotorsAfterHome();
findStartPosition();
} else if (command == "33") {
motor1Running = true;
motor2Running = true;
findStartPosition();
moveToPosition3();
reverseMotorsAfterHome();
findStartPosition();
} else if (command == "44") {
motor1Running = true;
motor2Running = true;
findStartPosition();
moveToPosition4();
reverseMotorsAfterHome();
findStartPosition();
} else if (command == "55") {
motor1Running = true;
motor2Running = true;
findStartPosition();
reverseMotorsAfterHome();
moveToPosition1();
moveMotorsHome();
findStartPosition();
} else if (command == "66") {
motor1Running = true;
motor2Running = true;
findStartPosition();
reverseMotorsAfterHome();
moveToPosition2();
moveMotorsHome();
findStartPosition();
} else if (command == "77") {
motor1Running = true;
motor2Running = true;
findStartPosition();
reverseMotorsAfterHome();
moveToPosition3();
moveMotorsHome();
findStartPosition();
} else if (command == "88") {
motor1Running = true;
motor2Running = true;
findStartPosition();
reverseMotorsAfterHome();
moveToPosition4();
moveMotorsHome();
findStartPosition();
}
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void calibrateSensors() {
Serial.println("Calibrating sensors...");
delay(1000);
Serial.println("Calibration complete.");
}
///////////////////////////////////////////////////////////////////////////////////
///
void Line_follower() {
distance = getDistance();
if (distance < stop_distance) {
Stop();
return;
}
if (digitalRead(sensorPin5) == LOW) {
forword();
delay(1000);
Vuong_goc();
delay(200);
unsigned long startTime = millis();
while (true) {
int sensorStates = (digitalRead(L_L_S) << 4) | (digitalRead(L_S) << 3) |
(digitalRead(S_S) << 2) | (digitalRead(R_S) << 1) | digitalRead(R_R_S);
if (sensorStates == 0b11110 || sensorStates == 0b11101 || sensorStates ==
0b11100) {
break;
}
if (millis() - startTime > 5000) {
break;
}
Vuong_goc();
delay(100);
}
Stop();
}
handleLineFollowing();
}
///////////////////////////////////////////////////////////////////////////////////
///
void handleLineFollowing() {
int sensorStates = (digitalRead(L_L_S) << 4) | (digitalRead(L_S) << 3) |
(digitalRead(S_S) << 2) | (digitalRead(R_S) << 1) | digitalRead(R_R_S);
if (sensorStates != 0b00000) {
lastValidSensorTime = millis();
}
if (millis() - lastValidSensorTime > 2000) {
Serial.println("No valid signal for 2 seconds, stopping.");
forword();
delay(200);
Stop();
lastValidSensorTime = millis();
}
int L_L_S_val = !digitalRead(L_L_S);
int L_S_val = !digitalRead(L_S);
int S_S_val = !digitalRead(S_S);
int R_S_val = !digitalRead(R_S);
int R_R_S_val = !digitalRead(R_R_S);

// Tính toán vị trí dựa trên trọng số


int position = (L_L_S_val * -2) + (L_S_val * -1) + (S_S_val * 0) + (R_S_val * 1)
+ (R_R_S_val * 2);
int error = 0 - position; // Trung tâm mong muốn là 0

// Hiển thị thông tin lên Serial Monitor


Serial.print("Position: ");
Serial.print(position);
Serial.print(" | Error: ");
Serial.println(error);

// Điều khiển robot với PID


PID(error);

delay(100); // Thời gian giữa các lần đọc cảm biến


}
///////////////////////////////////////////////////////////////////////////////////
///
void findStartPosition() {
while (true) {
int sensorStates = (digitalRead(L_L_S) << 4) | (digitalRead(L_S) << 3) |
(digitalRead(S_S) << 2) | (digitalRead(R_S) << 1) | digitalRead(R_R_S);
if (sensorStates == 0b00000) {
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Da den VT0!");
moveMotorsHome();
//reverseMotorsAfterHome();
Stop();
delay(1000);
break;
}
Line_follower();
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void moveToPosition1() {
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Di chuyen -- VT1");
while (true) {
Line_follower();
int sensor1 = digitalRead(sensorPin1);
if (sensor1 == HIGH) {
Serial.println("Đã đến vị trí 1!");
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Da den VT1!");
Stop();
break;
}
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void moveToPosition2() {
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Di chuyen -- VT2");
while (true) {
Line_follower();
int sensor2 = digitalRead(sensorPin2);
if (sensor2 == HIGH) {
Serial.println("Đã đến vị trí 2!");
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Da den VT2!");
Stop();
break;
}
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void moveToPosition3() {
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Di chuyen -- VT3");
while (true) {
Line_follower();
int sensor3 = digitalRead(sensorPin3);
if (sensor3 == HIGH) {
Serial.println("Đã đến vị trí 3!");
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Da den VT3!");
Stop();
break;
}
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void moveToPosition4() {
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Di chuyen -- VT4");
while (true) {
Line_follower();
int sensor4 = digitalRead(sensorPin4);
int sensor3 = digitalRead(sensorPin3);
if (sensor4 == HIGH && sensor3 == HIGH) {
Serial.println("Đã đến vị trí 4!");
lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Da den VT4!");
Stop();
break;
}
}
}
///////////////////////////////////////////////////////////////////////////////////
///
long getDistance() {
digitalWrite(TRIG_PIN, LOW);
delayMicroseconds(2);
digitalWrite(TRIG_PIN, HIGH);
delayMicroseconds(10);
digitalWrite(TRIG_PIN, LOW);

duration = pulseIn(ECHO_PIN, HIGH);


return duration * 0.034 / 2;
}
///////////////////////////////////////////////////////////////////////////////////
///
void forword() {
analogWrite(motorAspeed, 80);
analogWrite(motorBspeed, 80);
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, HIGH);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
}
///////////////////////////////////////////////////////////////////////////////////
///
void turnLeft() {
analogWrite(motorAspeed, 50);
analogWrite(motorBspeed, 150);
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, HIGH);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
}
///////////////////////////////////////////////////////////////////////////////////
///
void turnRight() {
analogWrite(motorAspeed, 150);
analogWrite(motorBspeed, 50);
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, HIGH);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
}
///////////////////////////////////////////////////////////////////////////////////
///
void Stop() {
analogWrite(motorAspeed, 0);
analogWrite(motorBspeed, 0);
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, LOW);
digitalWrite(motorB2, LOW);
}
///////////////////////////////////////////////////////////////////////////////////
///
void Vuong_goc() {
analogWrite(motorAspeed, 100);
analogWrite(motorBspeed, 100);
digitalWrite(motorA1, HIGH);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
}
void PID(int error) {
// Tính toán P, I, D
int P = error;
integral += error;
int D = error - lastError;
lastError = error;

// Tính giá trị PID


float Pvalue = Kp * P;
float Ivalue = Ki * integral;
float Dvalue = Kd * D;

// Tính toán tốc độ động cơ


float motorSpeed = Pvalue + Ivalue + Dvalue;

int motorSpeedA = baseSpeed + motorSpeed;


int motorSpeedB = baseSpeed - motorSpeed;

// Giới hạn tốc độ động cơ


if (motorSpeedA > maxSpeed) motorSpeedA = maxSpeed;
if (motorSpeedB > maxSpeed) motorSpeedB = maxSpeed;
if (motorSpeedA < -maxSpeed) motorSpeedA = -maxSpeed;
if (motorSpeedB < -maxSpeed) motorSpeedB = -maxSpeed;

// Hiển thị giá trị PID lên Serial Monitor


Serial.print("P: ");
Serial.print(P);
Serial.print(" | I: ");
Serial.print(integral);
Serial.print(" | D: ");
Serial.println(D);

// Điều khiển động cơ với tốc độ đã tính toán


if ((digitalRead(L_L_S) == 1) && (digitalRead(L_S) == 1) && (digitalRead(S_S) ==
1)&& (digitalRead(R_S) == 1)&& (digitalRead(R_R_S) == 1)){
speedControl(0, 0);}
else{
speedControl(motorSpeedA, motorSpeedB);
}
}

void speedControl(int speedA, int speedB) {


if (speedA >= 0 && speedB >= 0) {
forward(speedA, speedB);
}
else if (speedA < 0 && speedB >= 0) {
rightTurn(-speedA, speedB);
}
else if (speedA >= 0 && speedB < 0) {
leftTurn(speedA, -speedB);
}
}

void forward(int speedA, int speedB) {


digitalWrite(motorA1, HIGH);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
analogWrite(motorAspeed, speedA);
analogWrite(motorBspeed, speedB);
}

void rightTurn(int speedA, int speedB) {


digitalWrite(motorA1, LOW);
digitalWrite(motorA2, HIGH);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
analogWrite(motorAspeed, speedA);
analogWrite(motorBspeed, speedB);
}

void leftTurn(int speedA, int speedB) {


digitalWrite(motorA1, HIGH);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, LOW);
digitalWrite(motorB2, HIGH);
analogWrite(motorAspeed, speedA);
analogWrite(motorBspeed, speedB);
}

///////////////////////////////////////////////////////////////////////////////////
///
/*void displayLineSensorStatus() {
Serial.print("L_L_S: ");
Serial.print(digitalRead(L_L_S));
Serial.print(" | L_S: ");
Serial.print(digitalRead(L_S));
Serial.print(" | S_S: ");
Serial.print(digitalRead(S_S));
Serial.print(" | R_S: ");
Serial.print(digitalRead(R_S));
Serial.print(" | R_R_S: ");
Serial.print(digitalRead(R_R_S));
Serial.print(" | Sensor 1: ");
Serial.print(digitalRead(sensorPin1));
Serial.print(" | Sensor 2: ");
Serial.print(digitalRead(sensorPin2));
Serial.print(" | Sensor 3: ");
Serial.print(digitalRead(sensorPin3));
Serial.print(" | Sensor 4: ");
Serial.println(digitalRead(sensorPin4));
}*/
///////////////////////////////////////////////////////////////////////////////////
///
void rotateMotorsForward() {
if (digitalRead(LIMIT_SWITCH_TOP) == LOW) {
stopMotors();
return;
}
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}
///////////////////////////////////////////////////////////////////////////////////
///
void rotateMotorsBackward() {
if (digitalRead(LIMIT_SWITCH1) == LOW) {
stopMotors();
delay(1000);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
delay(6000);
stopMotors();
} else {
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void stopMotors() {
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
///////////////////////////////////////////////////////////////////////////////////
///
void checkLimitSwitch(int limitSwitchPin, int motorPin1, int motorPin2, String
motorName) {
if (digitalRead(limitSwitchPin) == LOW) {
digitalWrite(motorPin1, LOW);
digitalWrite(motorPin2, LOW);
}
}
///////////////////////////////////////////////////////////////////////////////////
///
void moveMotorsHome() {
unsigned long startMillis = millis();
bool motor1Home = false;
bool motor2Home = false;

lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Dang ha hang");

while ((millis() - startMillis < timeoutMillis) && (!motor1Home || !motor2Home))


{
if (!motor1Home && digitalRead(LIMIT_SWITCH1) == HIGH) {
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
} else {
motor1Home = true;
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
}
if (!motor2Home && digitalRead(LIMIT_SWITCH2) == HIGH) {
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
} else {
motor2Home = true;
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
if (digitalRead(LIMIT_SWITCH_TOP) == LOW) {
if (!(motor1Running && motor2Running)) {
stopMotors();
return;
}
}
if (millis() - startMillis >= timeoutMillis) {
break;
}
}
stopMotors();
}
///////////////////////////////////////////////////////////////////////////////////
///
void reverseMotorsAfterHome() {
rotateMotorsBackward();
delay(5000);
stopMotors();
}
///////////////////////////////////////////////////////////////////////////////////
///
void blinkLED() {
static unsigned long previousMillis = 0;
unsigned long currentMillis = millis();

lcd.clear();
lcd.setCursor(0, 0);
lcd.print("Dang nang hang");

if (currentMillis - previousMillis >= 1000) {


previousMillis = currentMillis;
static bool ledState = LOW;
ledState = !ledState;
digitalWrite(LED_PIN, ledState);
}
}

You might also like