Skip to content

Commit

Permalink
Update code
Browse files Browse the repository at this point in the history
  • Loading branch information
@ndhanh2711
ndhanh2711 committed Dec 23, 2024
1 parent 7cc6a5a commit d9458d1
Show file tree
Hide file tree
Showing 6 changed files with 2,191 additions and 2,147 deletions.
3 changes: 3 additions & 0 deletions .vscode/settings.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
{
"idf.portWin": "COM6"
}
17 changes: 9 additions & 8 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
# For more information about build system see
# https://docs.espressif.com/projects/esp-idf/en/latest/api-guides/build-system.html
# The following five lines of boilerplate have to be in your project's
# CMakeLists in this exact order for cmake to work correctly
cmake_minimum_required(VERSION 3.16)

include($ENV{IDF_PATH}/tools/cmake/project.cmake)
project(LINE-TRACKINGROBOT)
# For more information about build system see
# https://docs.espressif.com/projects/esp-idf/en/latest/api-guides/build-system.html
# The following five lines of boilerplate have to be in your project's
# CMakeLists in this exact order for cmake to work correctly
cmake_minimum_required(VERSION 3.16)

include($ENV{IDF_PATH}/tools/cmake/project.cmake)

project(LINE-TRACKINGROBOT)
79 changes: 79 additions & 0 deletions codeArduino/AlgoBasic/main.c
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,79 @@
#include "main.h"

// Hàm điều khiển tốc độ động cơ
void van_toc(int speed_motor1, int speed_motor2) {
ledcWrite(PWM_CHANNEL_1, constrain(speed_motor1, 0, 255));
ledcWrite(PWM_CHANNEL_2, constrain(speed_motor2, 0, 255));
}

// Hàm kiểm tra và điều chỉnh line
void kiem_tra_line() {
int sensorState[8];
for (int i = 0; i < 8; i++) {
int sensorValue = analogRead(vt_adc[i]);
sensorState[i] = (sensorValue > DK_adc[i]) ? 1 : 0; // 1: có line, 0: không có line
}

// Nếu cảm biến 4 và 5 bắt được line -> chạy thẳng
if (sensorState[3] == 1 && sensorState[4] == 1) {
van_toc(base_speed, base_speed);
}
// Nếu cảm biến khác bắt được line, điều chỉnh theo hệ số "a"
else {
int a = 0; // Hệ số tăng tốc
int slow_down = 0; // Hệ số giảm tốc

if (sensorState[0] || sensorState[1] || sensorState[2]) {
// Line nằm ở bên trái
if (sensorState[2]) {
a = max_a / 3;
slow_down = base_speed / 3; // Giảm nhẹ bánh không rẽ
} else if (sensorState[1]) {
a = (1 * max_a) / 2;
slow_down = base_speed / 3;
} else if (sensorState[0]) {
a = max_a;
slow_down = base_speed; // Giảm nhiều hơn để hỗ trợ rẽ gấp
}
van_toc(base_speed - a - slow_down, base_speed + a); // Quay trái
} else if (sensorState[5] || sensorState[6] || sensorState[7]) {
// Line nằm ở bên phải
if (sensorState[5]) {
a = max_a / 3;
slow_down = base_speed / 3; // Giảm nhẹ bánh không rẽ
} else if (sensorState[6]) {
a = (1 * max_a) / 2;
slow_down = base_speed / 3;
} else if (sensorState[7]) {
a = max_a;
slow_down = base_speed; // Giảm nhiều hơn để hỗ trợ rẽ gấp
}
van_toc(base_speed + a, base_speed - a - slow_down); // Quay phải
} else {
// Không có line -> dừng xe
van_toc(0, 50);
}
}
}

void setup() {
Serial.begin(9600);

// Cài đặt GPIO cảm biến
for (int i = 0; i < 8; i++) {
pinMode(vt_adc[i], INPUT);
}

// Cài đặt PWM cho động cơ
ledcSetup(PWM_CHANNEL_1, PWM_FREQ, PWM_RESOLUTION);
ledcAttachPin(MOTOR1_GPIO, PWM_CHANNEL_1);
ledcSetup(PWM_CHANNEL_2, PWM_FREQ, PWM_RESOLUTION);
ledcAttachPin(MOTOR2_GPIO, PWM_CHANNEL_2);

Serial.println("Hệ thống sẵn sàng.");
}

void loop() {
kiem_tra_line(); // Kiểm tra và điều chỉnh theo line
delay(2); // Tần số kiểm tra line
}
51 changes: 51 additions & 0 deletions codeArduino/AlgoBasic/main.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
#ifndef MAIN_H
#define MAIN_H


#include <Arduino.h>
#include <stdio.h>

/**
* Định nghĩa các chân của cảm biếnbiến
*/
#define INPUT_PIN_1 34
#define INPUT_PIN_2 35
#define INPUT_PIN_3 32
#define INPUT_PIN_4 33
#define INPUT_PIN_5 25
#define INPUT_PIN_6 26
#define INPUT_PIN_7 27
#define INPUT_PIN_8 14

/**
* Định nghĩa các chân của động cơ
*/
#define MOTOR1_GPIO 15
#define MOTOR2_GPIO 4
#define PWM_CHANNEL_1 0
#define PWM_CHANNEL_2 1
#define PWM_FREQ 5000
#define PWM_RESOLUTION 8

/**
* Giá trị ngưỡng của cảm biến sau khi tính toán
*/
int DK_adc[8] = {2022, 2384, 2388, 2559, 2233, 2254, 2559, 2553};

// Tốc độ cơ bản
int base_speed = 25;

// Hệ số điều chỉnh tốc độ
int max_a = 120; // Hệ số "a" lớn nhất

// Mảng GPIO cảm biến
int vt_adc[] = {INPUT_PIN_1, INPUT_PIN_2, INPUT_PIN_3, INPUT_PIN_4,
INPUT_PIN_5, INPUT_PIN_6, INPUT_PIN_7, INPUT_PIN_8};

void van_toc(int speed_motor1, int speed_motor2);

void kiem_tra_line();

void setup();

#endif // FILE_NAME_H
172 changes: 41 additions & 131 deletions main/main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,145 +2,55 @@
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/ledc.h"
#include "esp_adc_cal.h"
#include "esp_system.h"
#include "driver/gpio.h"
#include "driver/adc.h"
#include "esp_adc_cal.h"

// Định nghĩa các chân cảm biến
#define INPUT_PIN_1 GPIO_NUM_34
#define INPUT_PIN_2 GPIO_NUM_35
#define INPUT_PIN_3 GPIO_NUM_32
#define INPUT_PIN_4 GPIO_NUM_33
#define INPUT_PIN_5 GPIO_NUM_25
#define INPUT_PIN_6 GPIO_NUM_26
#define INPUT_PIN_7 GPIO_NUM_27
#define INPUT_PIN_8 GPIO_NUM_14

// Định nghĩa GPIO và PWM cho động cơ
#define MOTOR1_GPIO GPIO_NUM_15
#define MOTOR2_GPIO GPIO_NUM_4
#define PWM_CHANNEL_1 LEDC_CHANNEL_0
#define PWM_CHANNEL_2 LEDC_CHANNEL_1
#define PWM_FREQ 5000
#define PWM_RESOLUTION LEDC_TIMER_8_BIT

// Ngưỡng cảm biến
int DK_adc[8] = {2022, 2384, 2388, 2559, 2233, 2254, 2559, 2553};

// Tốc độ cơ bản
int base_speed = 64; // Đặt lại giá trị phù hợp với độ phân giải PWM 8-bit

// Hệ số điều chỉnh tốc độ
int max_a = 155; // Hệ số "a" lớn nhất cho độ phân giải PWM 8-bit

// Mảng GPIO cảm biến
int vt_adc[] = {INPUT_PIN_1, INPUT_PIN_2, INPUT_PIN_3, INPUT_PIN_4,
INPUT_PIN_5, INPUT_PIN_6, INPUT_PIN_7, INPUT_PIN_8};

// Hàm cấu hình GPIO cho cảm biến
void configure_gpio() {
gpio_config_t io_conf = {
.mode = GPIO_MODE_INPUT,
.pin_bit_mask = (1ULL << INPUT_PIN_1) | (1ULL << INPUT_PIN_2) | (1ULL << INPUT_PIN_3) |
(1ULL << INPUT_PIN_4) | (1ULL << INPUT_PIN_5) | (1ULL << INPUT_PIN_6) |
(1ULL << INPUT_PIN_7) | (1ULL << INPUT_PIN_8),
.pull_up_en = GPIO_PULLUP_DISABLE,
.pull_down_en = GPIO_PULLDOWN_ENABLE,
.intr_type = GPIO_INTR_DISABLE
};
gpio_config(&io_conf);
}
#include "esp_log.h"

// Hàm cấu hình PWM cho động cơ
void configure_pwm() {
ledc_timer_config_t ledc_timer = {
.duty_resolution = PWM_RESOLUTION,
.freq_hz = PWM_FREQ,
.speed_mode = LEDC_LOW_SPEED_MODE,
.timer_num = LEDC_TIMER_0
};
ledc_timer_config(&ledc_timer);
#define INPUT_PIN_1 34
#define INPUT_PIN_2 35
#define INPUT_PIN_3 32
#define INPUT_PIN_4 33
#define INPUT_PIN_5 25
#define INPUT_PIN_6 26
#define INPUT_PIN_7 27
#define INPUT_PIN_8 14

ledc_channel_config_t motor1_channel = {
.channel = PWM_CHANNEL_1,
.duty = 0,
.gpio_num = MOTOR1_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.hpoint = 0,
.timer_sel = LEDC_TIMER_0
};
ledc_channel_config(&motor1_channel);
static const char *TAG = "ADC_INPUT";

ledc_channel_config_t motor2_channel = {
.channel = PWM_CHANNEL_2,
.duty = 0,
.gpio_num = MOTOR2_GPIO,
.speed_mode = LEDC_LOW_SPEED_MODE,
.hpoint = 0,
.timer_sel = LEDC_TIMER_0
};
ledc_channel_config(&motor2_channel);
}

// Hàm điều khiển tốc độ động cơ
void van_toc(int speed_motor1, int speed_motor2) {
ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_CHANNEL_1, speed_motor1);
ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_CHANNEL_1);

ledc_set_duty(LEDC_LOW_SPEED_MODE, PWM_CHANNEL_2, speed_motor2);
ledc_update_duty(LEDC_LOW_SPEED_MODE, PWM_CHANNEL_2);
}

// Hàm kiểm tra và điều chỉnh line
void kiem_tra_line() {
int sensorState[8];
for (int i = 0; i < 8; i++) {
int sensorValue = adc1_get_raw(vt_adc[i]);
sensorState[i] = (sensorValue > DK_adc[i]) ? 1 : 0; // 1: có line, 0: không có line
}

if (sensorState[3] == 1 && sensorState[4] == 1) {
van_toc(base_speed, base_speed);
} else {
int a = 0; // Hệ số tăng tốc
int slow_down = 0; // Hệ số giảm tốc

if (sensorState[0] || sensorState[1] || sensorState[2]) {
if (sensorState[2]) {
a = max_a / 3;
slow_down = base_speed / 3;
} else if (sensorState[1]) {
a = (2 * max_a) / 3;
slow_down = base_speed / 3;
} else if (sensorState[0]) {
a = max_a;
slow_down = 2 * base_speed / 3;
}
van_toc(base_speed - a - slow_down, base_speed + a);
} else if (sensorState[5] || sensorState[6] || sensorState[7]) {
if (sensorState[5]) {
a = max_a / 3;
slow_down = base_speed / 3;
} else if (sensorState[6]) {
a = (2 * max_a) / 3;
slow_down = base_speed / 3;
} else if (sensorState[7]) {
a = max_a;
slow_down = 2 * base_speed / 3;
}
van_toc(base_speed + a, base_speed - a - slow_down);
} else {
van_toc(0, 0);
}
}
}
// Cấu hình kênh ADC cho các chân
#define ADC_WIDTH ADC_WIDTH_BIT_12 // Độ phân giải 12 bit (dải từ 0-4095)
#define ADC_ATTEN ADC_ATTEN_DB_0 // Attenuation, mức độ giảm tín hiệu vào

void app_main() {
configure_gpio();
configure_pwm();
// Cấu hình các chân GPIO làm đầu vào ADC
adc1_config_width(ADC_WIDTH); // Cấu hình độ phân giải
adc1_config_channel_atten(ADC1_CHANNEL_0, ADC_ATTEN); // Cấu hình kênh cho chân GPIO 34
adc1_config_channel_atten(ADC1_CHANNEL_3, ADC_ATTEN); // Chân GPIO 35
adc1_config_channel_atten(ADC1_CHANNEL_4, ADC_ATTEN); // Chân GPIO 32
adc1_config_channel_atten(ADC1_CHANNEL_5, ADC_ATTEN); // Chân GPIO 33
adc1_config_channel_atten(ADC1_CHANNEL_6, ADC_ATTEN); // Chân GPIO 25
adc1_config_channel_atten(ADC1_CHANNEL_7, ADC_ATTEN); // Chân GPIO 26
adc1_config_channel_atten(ADC2_CHANNEL_8, ADC_ATTEN); // Chân GPIO 27
adc1_config_channel_atten(ADC2_CHANNEL_9, ADC_ATTEN); // Chân GPIO 14

while (1) {
kiem_tra_line();
vTaskDelay(pdMS_TO_TICKS(1)); // Tần số kiểm tra line
// Đọc giá trị từ các chân ADC (dải từ 0 đến 4095)
int value_1 = adc1_get_raw(ADC1_CHANNEL_0); // GPIO 34
int value_2 = adc1_get_raw(ADC1_CHANNEL_3); // GPIO 35
int value_3 = adc1_get_raw(ADC1_CHANNEL_4); // GPIO 32
int value_4 = adc1_get_raw(ADC1_CHANNEL_5); // GPIO 33
int value_5 = adc1_get_raw(ADC1_CHANNEL_6); // GPIO 25
int value_6 = adc1_get_raw(ADC1_CHANNEL_7); // GPIO 26
int value_7 = adc1_get_raw(ADC2_CHANNEL_8); // GPIO 27
int value_8 = adc1_get_raw(ADC2_CHANNEL_9); // GPIO 14

// In ra giá trị của từng chân ADC
ESP_LOGI(TAG, "Pin 1: %d\n, Pin 2: %d\n, Pin 3: %d\n, Pin 4: %d\n, Pin 5: %d\n, Pin 6: %d\n, Pin 7: %d\n, Pin 8: %d\n\n\n",
value_1, value_2, value_3, value_4, value_5, value_6, value_7, value_8);

// Delay cho lần đọc tiếp theo
vTaskDelay(pdMS_TO_TICKS(500));
}
}
Loading

0 comments on commit d9458d1

Please sign in to comment.