Revision 11:8d71a1e1e947, committed 2019-08-09

Comitter:

amaral99

Date:

Fri Aug 09 17:11:40 2019 +0000

Parent:

10:e412756d0f66

Commit message:

.

Changed in this revision

--- a/Motor/Motor.cpp	Tue Jun 04 20:19:19 2019 +0000
+++ b/Motor/Motor.cpp	Fri Aug 09 17:11:40 2019 +0000
@@ -12,44 +12,46 @@
 //################################################################################
 //****************************  DEFINICOES - INICIO  *****************************
 
-Ticker Left;  //Criacao do objeto do motor da esquerda
-Ticker Right; //Criacao do objeto do motor da direita
+PwmOut LeftMotor(p23);  //Definicao da porta do motor da esquerda
+PwmOut RightMotor(p24); //Definicao da porta do motor da direita
 
-DigitalOut LeftMotor(p17);  //Definicao da porta do motor da esquerda
-DigitalOut RightMotor(p16); //Definicao da porta do motor da direita
+#define MIN  990
+#define MAX  2040
+#define MID ((MAX+MIN)/2)
 
-#define MIN  1000
-#define STOP 1500
-#define MAX  2000
+int RLeft, RRight;
 
 //*****************************  DEFINICOES - FINAL  *****************************
 //################################################################################
 //################################################################################
 //******************************* FUNCOES - INICIO  ******************************
 
-//Funcao que inicializa os motores
-void Motors_Setup() { LeftMotor  = 0; RightMotor = 0; }
-
 //Funcao que gera o sinal PPM do motor da esquerda
-void Set_Left()  { LeftMotor  = !LeftMotor;  }
+void Set_Left(int pulse)  { LeftMotor.pulsewidth_us(pulse);  }
 
 //Funcao que gera o sinal PPM do motor da direita
-void Set_Right() { RightMotor = !RightMotor; }
+void Set_Right(int pulse) { RightMotor.pulsewidth_us(pulse); }
+
+//Funcao que inicializa os motores
+void Motors_Setup(void) { RightMotor.period_ms(16); LeftMotor.period_ms(16); Set_Left(MID); Set_Right(MID); }//original 16
 
 //Funcao que desloca os motores de acordo com o sinal PPM recebido
-void Drive(float LeftSpeed, float RightSpeed) {
+void Drive(int LeftSpeed, int RightSpeed) {
     
-    if (LeftSpeed < 0) { LeftSpeed = (STOP - abs(LeftSpeed)*5)/1000000; } 
-    else { LeftSpeed = (STOP + abs(LeftSpeed)*5)/1000000; }
+    if (LeftSpeed < 0) { LeftSpeed = (MID - abs(LeftSpeed)*5); } //original 5
+    else { LeftSpeed = (MID + abs(LeftSpeed)*5); } //original 5
         
-    if (RightSpeed < 0) { RightSpeed = (STOP - abs(RightSpeed)*5)/1000000; } 
-    else { RightSpeed = (STOP + abs(RightSpeed)*5)/1000000; }
+    if (RightSpeed < 0) { RightSpeed = (MID - abs(RightSpeed)*5); } //original 5
+    else { RightSpeed = (MID + abs(RightSpeed)*5); } //original 5
     
-    Left.attach (&Set_Left , LeftSpeed);
-    Right.attach(&Set_Right, RightSpeed);
+    Set_Left(LeftSpeed); Set_Right(RightSpeed);
+    RLeft = LeftSpeed; RRight = RightSpeed;
+    
 }
 
-void Stop(void){  
-    while(1){ Drive(STOP, STOP); } 
-    }
+int Get_Left(void) { return RLeft; }
+int Get_Right(void) { return RRight; }
+
+//Funcao que matem o robo parado infinitamente
+void Stop(void){ while(1){ Motors_Setup(); } }
 //******************************** FUNCOES - FINAL  ******************************
\ No newline at end of file

--- a/Motor/Motor.h	Tue Jun 04 20:19:19 2019 +0000
+++ b/Motor/Motor.h	Fri Aug 09 17:11:40 2019 +0000
@@ -2,8 +2,10 @@
 //==========================  MOTORS CONTROL - MOTOR.H  ==========================
 //================================================================================
 
+void Set_Left       (int pulse);
+void Set_Right      (int pulse);
 void Motors_Setup   (void);
-void Set_Left       (void);
-void Set_Right      (void);
-void Drive          (float LeftSpeed, float RightSpeed);
-void Stop           (void);
\ No newline at end of file
+void Drive          (int LeftSpeed, int RightSpeed);
+void Stop           (void);
+int Get_Left(void);
+int Get_Right(void);
\ No newline at end of file

--- a/PIDControl/PID.cpp	Tue Jun 04 20:19:19 2019 +0000
+++ b/PIDControl/PID.cpp	Fri Aug 09 17:11:40 2019 +0000
@@ -14,20 +14,22 @@
 //################################################################################  
 //****************************  DEFINICOES - INICIO  *****************************
 
-#define Kp 0.5  //Definicao da constante proporcional do PID - Melhor valor 0.5
-#define Kd 8.812  //Definicao da constante derivativa   do PID - Melhor valor 5.0
-#define Ki -0.03  //Definicao da constante integrativa  do PID
-    
+#define Kp 0.05  //Definicao da constante proporcional do PID - Melhor valor 0.5
+#define Kd 08.812 //Definicao da constante derivativa   do PID - Melhor valor 8.812
+#define Ki -0.03  //Definicao da constante integrativa  do PID - Melhor valor -0.03
+
 #define MIN_SPEED   0.0 //Definicao da velocidade minima em %
-#define BASE_SPEED 30.0 //Definicao da velocidade base (no SetPoint) em %
-#define MAX_SPEED  50.0 //Definicao da velocidade maxima em %
+#define BASE_SPEED 20.0 //Definicao da velocidade base (no SetPoint) em %
+#define MAX_SPEED  80.0 //Definicao da velocidade maxima em %
 
-#define GANHO_CURVA 25 //Definicao do ganho em curva (quando o LF perder a linha) Valor original: 32
+#define GANHO_CURVA 10 //Definicao do ganho em curva (quando o LF perder a linha) Valor original: 32
 #define SET_POINT   45 //Definicao da leitura no SetPoint
 
 #define MIN_INTEGRAL -50 //Definicao do valor minimo da integral do PID
 #define MAX_INTEGRAL +50 //Definicao do valor maximo da integral do PID
 
+Serial PCPID(USBTX,USBRX);
+
 int LastError  = 0; //Definicao da variavel que armazenara o ultimo erro
 int LastSensor = 0; //Definicao da variavel que armazenara a ultima posicao
 int Integral   = 0; //Definicao  da variavel que armazera a soma dos erros
@@ -47,11 +49,13 @@
     
     float Control; //Inicializacao da variavel de controle
     
-    //Calculo do controle a partir do Erro
     Error = Error - SET_POINT;
-    if(Error < 30 && Error > -30){ Control = (Error * Kp) + ((Error - LastError) * Kd) + (Integral * Ki); }
-    else { Control = (Error * 2 * Kp) + ((Error - LastError) * 2 * Kd) + (Integral * Ki); }
+    PCPID.printf("Error: %d\n\r",Error);
     
+    //Calculo do controle a partir do Erro\ Error = Error;
+    if(Error > 25 || Error < -25){ Control = (Error * 5 * Kp) + ((Error - LastError)* 5 * Kd) + (Integral* 5 * Ki); }
+    else { Control = (Error * Kp) + ((Error - LastError) * Kd) + (Integral * Ki); }
+    PCPID.printf("Control: %d\n\r",Control);
     LastError = Error; //Atualizacao do ultimo Erro
     Integral += Error; //Atualizacao da samoa dos Erros
     

--- a/main.cpp	Tue Jun 04 20:19:19 2019 +0000
+++ b/main.cpp	Fri Aug 09 17:11:40 2019 +0000
@@ -10,6 +10,8 @@
 #include "Motor.h"
 #include "Sensor.h"
 
+Serial bt(p28, p27);
+
 //Definição das variaveis para as velocidades dos motores
 float LMotor = 0;
 float RMotor = 0;
@@ -31,6 +33,10 @@
 //################################################################################
 //################################################################################
 //******************************* FUNCOES - INICIO  ******************************
+void bluetooth (float left, float right) {
+    bt.printf("%d, %.2f, %.2f, %d, %d\n\r", Get_Position(), left, right,Get_Left(),Get_Right());  
+      
+}
 
 //Funcao que inicializa a interrupcao do sensor lateral direito
 
@@ -47,7 +53,7 @@
     led4 = 0;
     #endif
     
-    END.attach(&Stop, 20.0); //Inicia END para executar a função Stop depois de 20s
+    //END.attach(&Stop, 20.0); //Inicia END para executar a função Stop depois de 20s
     
     //Loop infinito
     while (1) { // Valor correto: 17/18
@@ -64,8 +70,9 @@
             }*/
         Read_Sensors();                                 //Lê os sensores
         PID_Control(Get_Position(), &LMotor, &RMotor);  //Calcula o controle
-        Drive(LMotor, RMotor);                          //Passa os valores pro motor
-        wait(0.0035);                                   //Se não tiver delay motor fica sem controle
+        Drive(LMotor, RMotor); 
+        bluetooth(LMotor,RMotor);                         //Passa os valores pro motor
+        //wait(0.0035);                                   //Se não tiver delay motor fica sem controle
     } 
     
     /*for (i=0; i < 4200; i++) {
@@ -75,7 +82,7 @@
     }
     Stop();*/  
     
-    return 1;
+    return 0;
 }
 
 //******************************** FUNCOES - FINAL  ******************************
\ No newline at end of file