diff --git a/Test_motor/include/MotorDC.h b/Test_motor/include/MotorDC.h index 16387d0..fdf8c39 100644 --- a/Test_motor/include/MotorDC.h +++ b/Test_motor/include/MotorDC.h @@ -15,13 +15,15 @@ class MotorDC{ void ligar_motor(int direcao, int pwmVal); void ler_encoder(); void resetar_encoder(); - void andar_reto(int velocidade_rpm); + void set_RPM(int velocidade_rpm); volatile double posi; // posição do motor em ticks do encoder double rps = 0; // velocidade ATUAL do motor em rotações por segundo - int encoder_volta; // valor de encoder referente a uma volta completa da roda - double comprimento_roda = 2 * M_PI * 6.272; //TODO: medir o raio da roda real + double get_rpm() { return (rps*60); } // função para obter a velocidade do motor em rpm - private: + private: + Tempo tempo; + int encoder_volta; // valor de encoder referente a uma volta completa da roda + double comprimento_roda = 2 * M_PI * 0.000000016; int ENCA; // Cabo amarelo int ENCB; // Cabo branco int PWM; @@ -33,11 +35,10 @@ class MotorDC{ float ki; // constante integral do controle PID float kd; // constante derivativa do controle PID int rpm_referencia; // velocidade desejada do motor, velocidade que ele buscará alcançar - double rpm_max = 87; // velocidade máxima do motor (apenas por curiosidade, usar caso seja necessário) float eprev = 0; float eintegral = 0; // erro acumulado pro cálculo do ki int dir = 1; // 1 para frente, -1 para trás (pelo menos essa é a ideia) - + }; #endif \ No newline at end of file diff --git a/Test_motor/include/Pins.h b/Test_motor/include/Pins.h index 8049845..78f1b1e 100644 --- a/Test_motor/include/Pins.h +++ b/Test_motor/include/Pins.h @@ -6,26 +6,28 @@ // Pinos dos motores -#define ENCA1 2 -#define ENCB1 4 -#define IN2 13 -#define IN1 15 +#define ENCA_E 15 +#define ENCB_E 2 +#define IN2 26 +#define IN1 25 -#define ENCA2 5 -#define ENCB2 18 +#define ENCA_D 4 +#define ENCB_D 18 #define IN4 14 #define IN3 12 #define LEDON 33 -#define D1 26 -#define D2 25 +#define D1 33 +#define D2 32 #define D3 16 #define D4 17 #define D5 19 #define D6 21 #define D7 22 #define D8 23 +#define D1_2 13 +#define D2_2 27 #endif \ No newline at end of file diff --git a/Test_motor/include/Tempo.h b/Test_motor/include/Tempo.h index 5665081..f16d89d 100644 --- a/Test_motor/include/Tempo.h +++ b/Test_motor/include/Tempo.h @@ -1,13 +1,18 @@ -#ifndef Tempo_h -#define Tempo_h +// Tempo.h +#ifndef TEMPO_H +#define TEMPO_H -#include "Arduino.h" +#include -//* Este arquivo contém a declaração das variáveis de tempo utilizadas no projeto -extern unsigned long T; -extern unsigned long prevT; -extern double dt; +class Tempo { + private: + unsigned long prevMicros; + double dt; // em segundos -void atualizar_tempo(); + public: + Tempo(); // Construtor + void atualizar(); // Atualiza o tempo atual e calcula o dt + double getDeltaTime(); // Retorna o dt atual +}; -#endif \ No newline at end of file +#endif diff --git a/Test_motor/platformio.ini b/Test_motor/platformio.ini index 0aa6fde..680af93 100644 --- a/Test_motor/platformio.ini +++ b/Test_motor/platformio.ini @@ -18,12 +18,6 @@ platform = espressif32 board = esp32dev framework = arduino -monitor_speed = 9600 +monitor_speed = 115200 upload_protocol = esptool - - -;Comando Mario Ubuntu -;sudo modprobe -r usbserial -;sudo modprobe usbserial -;sudo modprobe ch341 \ No newline at end of file diff --git a/Test_motor/src/MotorDC.cpp b/Test_motor/src/MotorDC.cpp index 3ddd764..d7984af 100644 --- a/Test_motor/src/MotorDC.cpp +++ b/Test_motor/src/MotorDC.cpp @@ -16,6 +16,7 @@ MotorDC::MotorDC(const int ENCA, const int ENCB, const int IN1, const int IN2) pinMode(ENCA, INPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT); + } @@ -52,16 +53,8 @@ void MotorDC::ligar_motor(int direcao, int pwmVal){ // Função para ler o encoder do motor void MotorDC::ler_encoder(){ - - //fabs(rps*60)<1?posi+=dir:(rps*60)>1?posi++:posi--; - - if(fabs(rps * 60) < 1) - posi += dir; - else if ((rps * 60) > 1) - posi++; - else - posi--; - + posi++; + } // Função para resetar o encoder do motor @@ -71,62 +64,41 @@ void MotorDC::resetar_encoder() eintegral = 0; voltas = 0; } +void MotorDC::set_RPM(int velocidade_rpm){ -void MotorDC::andar_reto(int velocidade_rpm){ + //Serial.print("encoder_volta: "); Serial.println(encoder_volta); - // atualizar_tempo(); + tempo.atualizar(); + double dt = tempo.getDeltaTime(); - rpm_referencia = velocidade_rpm; // Velocidade de referência + rpm_referencia = fabs(velocidade_rpm); - volatile double posi_atual = 0; // posição atual do encoder - noInterrupts(); // desabilita interrupções - posi_atual = posi; // atualiza a posição atual do encoder - interrupts(); // reabilita interrupções + volatile double posi_atual = 0; + noInterrupts(); + posi_atual = posi; + interrupts(); - voltas_anterior = voltas; // atualiza o número de voltas anterior + voltas_anterior = voltas; + voltas = posi_atual / encoder_volta; + rps = (voltas - voltas_anterior) / dt; + rps = rps/100; - voltas = posi_atual / encoder_volta; // calcula o número de voltas do motor - rps = (voltas - voltas_anterior) / dt; // calcula a velocidade do motor em rps - - double e = rpm_referencia - (rps * 60); // calcula o erro da velocidade em rpm + double rpm = rps * 60; + double e = rpm_referencia - rpm; float p = kp * e; - eintegral += e; + float i = ki * eintegral * dt; + float d = kd * (e - eprev) / dt; + float u = p + i + d; - float i = ki * eintegral*dt; - - float d = kd * ((e - eprev) / dt); + float pwmVal = constrain(fabs(u), 0, 255); - float u = p + i + d; //p + (ki * eintegral*dt) + d; - - float pwmVal = fabs(u); // valor do pwm que será enviado ao motor - - if (pwmVal > 255) // Limita o valor do pwm para 255 - { - pwmVal = 255; - } - - // Define a direção do motor com base no valor de u - if (u > 0) - { - dir = 1; - } - else if (u < 0) - { - dir = -1; - } - else - { - dir = 0; - } - - if (velocidade_rpm != 0) { - ligar_motor(dir, pwmVal); - } else { - ligar_motor(0, 0); - } + if (velocidade_rpm > 0) dir = 1; + else if (velocidade_rpm < 0) dir = -1; + else dir = 0; + ligar_motor(dir, pwmVal); eprev = e; - -} \ No newline at end of file + +} diff --git a/Test_motor/src/Tempo.cpp b/Test_motor/src/Tempo.cpp index 5911a4e..f5f2c50 100644 --- a/Test_motor/src/Tempo.cpp +++ b/Test_motor/src/Tempo.cpp @@ -1,13 +1,17 @@ +// Tempo.cpp #include "Tempo.h" -extern unsigned long T; -extern unsigned long prevT; -extern double dt; +Tempo::Tempo() { + prevMicros = micros(); + dt = 0.0; +} +void Tempo::atualizar() { + unsigned long currentMicros = micros(); + dt = (currentMicros - prevMicros) / 1.0e6; // converte para segundos + prevMicros = currentMicros; +} -void atualizar_tempo() -{ - T = micros(); - dt = ((float) (T - prevT))/( 1.0e6 ); - prevT = T; -} \ No newline at end of file +double Tempo::getDeltaTime() { + return dt; +} diff --git a/Test_motor/src/main.cpp b/Test_motor/src/main.cpp index b3d74df..c72284b 100644 --- a/Test_motor/src/main.cpp +++ b/Test_motor/src/main.cpp @@ -1,88 +1,197 @@ #include #include -#include "MotorDC.h" -#include "Pins.h" +#include "MotorDC.h" // Certifique-se que esta biblioteca e a MotorDC.cpp estão no seu projeto +#include "Pins.h" // Certifique-se que este arquivo com as definições dos pinos está no seu projeto -// QTRSensors object +// Objeto do sensor QTR QTRSensors qtr; +QTRSensors Perifericos; -// Motor objects -MotorDC MotorD(ENCA1, ENCB1, IN2, IN1); // Left motor -MotorDC MotorE(ENCA2, ENCB2, IN3, IN4); // Right motor +// Objetos dos motores +//MotorDC MotorE(ENCA_E , ENCB_E , IN2, IN1); // Motor esquerdo +//MotorDC MotorD(ENCA_D , ENCB_D , IN3, IN4); // Motor direito -const uint8_t SensorCount = 8; // Number of sensors -uint16_t sensorValues[SensorCount]; // Array to store sensor values +const uint8_t SensorCount1 = 8; // Número de sensores no array principal +uint16_t sensorValues1[SensorCount1]; // Valores dos sensores no array principal +const uint8_t SensorCount2 = 2; // Número de sensores no array periférico +uint16_t sensorValues2[SensorCount2]; // Valores dos sensores no array periférico -void setup() -{ - // Configure the LEDON pin - pinMode(LEDON, OUTPUT); - digitalWrite(LEDON, HIGH); // Turn on the IR LEDs +int maxSpeed = 255; // Velocidade máxima em linha reta +int minSpeed = 35; // Velocidade mínima em curvas + +// Variáveis do controle PID +int error = 0; +int lastError = 0; +float KP = 1.2; // 1.3 e base em 80 +float KD = 3; // Ganho para suavizar as curvas! +float KI = 0; // Ganho para suavizar as retas! (Não utilizado, efetivamente um PD) + + + +/* + +Melhores runs: + + +kp 1.2 kd 5.0 BS= 80 - MotorD.ligar_motor(0,0); - MotorE.ligar_motor(0,0); - // Configure the sensors on pins D1 to D8 - qtr.setTypeRC(); - qtr.setSensorPins((const uint8_t[]){D1, D2, D3, D4, D5, D6, D7, D8}, SensorCount); +kp 2 kd 10 bs= 100; - Serial.begin(9600); - //delay(2000); - // Calibrate the sensors - Serial.println("Calibrating sensors..."); - for (uint16_t i = 0; i < 400; i++) + +*/ + +int cnt=0; +int currentBaseSpeed = 80; +bool marcadores[16] = {false,false,false,false,false,false,false,false, + false,false,false,false,false,false,false,false}; +bool brancoDetectado = false; + + + void setup() { - qtr.calibrate(); - delay(10); - } - Serial.println("Calibration complete."); -} -// PID control variables -int error = 0; -int lastError = 0; // To store the previous error -float KP = 0.1; // Proportional gain -float KD = 5; // Derivative gain -int baseSpeed = 100; // Base motor speed - -void loop() -{ - // Read the line position (0 to 7000 for 8 sensors) - uint16_t position = qtr.readLineBlack(sensorValues); - - // Calculate the error (center is 3500 for 8 sensors) - error = position - 3500; - - // Calculate motor speed adjustments using proportional and derivative control - int motorSpeed = KP * error + KD * (error - lastError); - - // Update lastError for the next iteration - lastError = error; - - // Set motor speeds - int leftMotorSpeed = baseSpeed - motorSpeed; - int rightMotorSpeed = baseSpeed + motorSpeed; - - // Constrain motor speeds to valid range (0 to 255) - leftMotorSpeed = constrain(leftMotorSpeed, 0, 100); - rightMotorSpeed = constrain(rightMotorSpeed, 0, 100); - - // Drive the motors - MotorE.ligar_motor(-1, leftMotorSpeed); - MotorD.ligar_motor(-1, rightMotorSpeed); - - // Optional: Print debug information - Serial.print("Position: "); - Serial.print(position); - Serial.print(" Error: "); - Serial.print(error); - Serial.print(" Left Speed: "); - Serial.print(leftMotorSpeed); - Serial.print(" Right Speed: "); - Serial.println(rightMotorSpeed); - delay(10); // Small delay for stability + // Liga os LEDs IR dos sensores + pinMode(LEDON, OUTPUT); + digitalWrite(LEDON, HIGH); + + // Define os pinos dos sensores + qtr.setTypeRC(); //Definição dos sensores principais + Perifericos.setTypeRC(); //Definição dos sensores periféricos + qtr.setSensorPins((const uint8_t[]){D1, D2, D3, D4, D5, D6, D7, D8}, SensorCount1); //declaração dos pinos dos sensores principais + Perifericos.setSensorPins((const uint8_t[]){D1_2, D2_2}, SensorCount2); //declaração dos pinos dos sensores periféricos + + Serial.begin(115200); + delay(2000); // Espera para abrir o Monitor Serial, se necessário + + // Calibração dos sensores + Serial.println("Calibrando sensores..."); + for (uint16_t i = 0; i < 400; i++) + { + qtr.calibrate(); + delay(10); + } + Serial.println("Calibração concluída."); + Serial.println("------------------------------------"); + Serial.println("Iniciando loop principal..."); + + } + + + void loop(){ + + uint16_t position = qtr.readLineWhite(sensorValues1); + + // Calcula o erro em relação ao centro (3500) + int error = map(position, 0, 7000, -100, 100); + + int derivative = error - lastError; + + int normal = abs(error) / 100; + currentBaseSpeed *= (1-normal); // Base reduzida proporcionalmente ao erro + currentBaseSpeed = constrain(currentBaseSpeed, 40, maxSpeed); + + // Correção com PID (PD) + int correction = KP * error + KD * derivative + KI * ((error + lastError) / 2); + + // Calcula as velocidades dos motores usando a currentBaseSpeed + int leftSpeed = currentBaseSpeed + (correction); + int rightSpeed = currentBaseSpeed - (correction); + + + // Liga os motores com as velocidades ajustadas + //MotorE.ligar_motor(1, rightSpeed); // Assumindo que '1' é para frente + //MotorD.ligar_motor(1, leftSpeed); // Assumindo que '1' é para frente + + // Atualiza o erro anterior + lastError = error; + + // Imprime dados para depuração") + Serial.print(" | BaseSpd: "); // Nova informação para depuração + Serial.print(currentBaseSpeed); + Serial.print(" | VE: "); + Serial.print(leftSpeed); + Serial.print(" | VD: "); + Serial.println(rightSpeed); + Serial.print(" | cnt value: "); + Serial.println(cnt); + + //Faz a leitura dos sensores periféricos, se ele perceber a leitura de um ele somará no contador + + Perifericos.read(sensorValues2, QTRReadMode::On); + + Perifericos.read(sensorValues2, QTRReadMode::On); + + for (int i = 0; i < SensorCount2; i++) { + if (sensorValues2[i] < 300) { + brancoDetectado = true; + } + + else if (brancoDetectado && sensorValues2[i] >= 2500) { + cnt++; + brancoDetectado = false; + } + } + + // Printa os valores dos sensores D1_2 e D2_2 + Serial.print("Leitura D1_2: "); + Serial.println(sensorValues2[0]); // D1_2 está no índice 0 + Serial.print("Leitura D2_2: "); + Serial.println(sensorValues2[1]); // D2_2 está no índice 1 + + // Garante que o código só execute uma vez por marca +if (!marcadores[cnt - 1]) { + marcadores[cnt - 1] = true; + Serial.print(">> Marca "); + Serial.print(cnt); + Serial.print(" detectada: "); + + switch (cnt) { + case 1: + currentBaseSpeed -= 20; + Serial.println("Ajustando velocidade."); + break; + case 2: + currentBaseSpeed -= 20; + Serial.println("Ajustando velocidade."); + break; + case 4: + currentBaseSpeed += 40; + Serial.println("Aumentando velocidade."); + break; + case 5: + currentBaseSpeed -= 30; + Serial.println("Ajustando velocidade."); + break; + case 6: + currentBaseSpeed += 20; + Serial.println("Aumentando velocidade."); + break; + case 10: + currentBaseSpeed -= 20; + Serial.println("Ajustando velocidade."); + break; + case 13: + currentBaseSpeed += 30; + Serial.println("Aumentando velocidade."); + break; + case 15: + currentBaseSpeed -= 20; + Serial.println("Ajustando velocidade."); + break; + case 16: + currentBaseSpeed += 20; + Serial.println("Aumentando velocidade."); + break; + default: + // Casos 3, 7, 8, 9, 11, 12, 14 não fazem nada + Serial.println("Velocidade mantida."); + break; + } +} -} \ No newline at end of file + delay(15); // Pequeno delay para estabilidade e leitura serial + } \ No newline at end of file