Diff: main.cpp

Revision:

2:027fe033f5f7

Parent:

1:584ac95c4061

diff -r 584ac95c4061 -r 027fe033f5f7 main.cpp
--- a/main.cpp	Thu Jun 29 20:59:25 2017 +0000
+++ b/main.cpp	Wed Jul 05 15:50:40 2017 +0000
@@ -3,8 +3,8 @@
 * @file    main.cpp
 * @author  Kleber Lima da Silva (kleber@ioton.cc)
 * @version V0.0.1
-* @date    19-Junho-2017
-* @brief   Programa para testes do hardware do Robô TON-BOT.
+* @date    05-Julho-2017
+* @brief   Programa de exemplo do Robô TON-BOT no modo seguidor de linha.
 ******************************************************************************
 * @attention
 *
@@ -27,36 +27,25 @@
 /* Includes ------------------------------------------------------------------*/
 #include "main.h"
 
-/** @addtogroup Teste TON-BOT
+/** @addtogroup Seguidor de linha TON-BOT
  * @{
  */
 
 /* Variáveis privadas --------------------------------------------------------*/
-Ticker imuTick;   /* Timer para o algoritmo da IMU */
-Timer t1;         /* Timer auxiliar */
 
 
 /* Funções Privadas ----------------------------------------------------------*/
 
 /**
- * @brief  Callback para execução do Algoritmo AHRS com taxa de 10 Hz
- * @note   O algoritmo AHRS calcula os ângulos: roll, pitch and yaw
- * @param  Nenhum
- * @retval Nenhum
- */
-void imuCallback(void)
-{
-//  imu.runAHRS(0.1); // ### FIX: TICKER com bug nesta versao do mbed
-} // fim da imuCallback
-
-/**
  * @brief  Programa Principal
  * @param  Nenhum
  * @retval Nenhum
  */
 int main(void)
 {
-  float lf = 0, l = 0, r = 0, rf = 0;
+  const float KP = 0.008, PWM_MEDIO = 0.3;
+  int32_t erro = 0, erro_anterior = 0;
+  float MV = 0;
 
   /* Ligue o TON com o botão USER pressionado para indicar o estado da bateria */
   if (ton.USERisPressed())
@@ -65,79 +54,40 @@
     ton.batteryStatus();
   }
 
-  /* Inicialização dos periféricos */
-  ton.setLED(BLUE);
-  initTonBot();
-  ton.setLED(RED);
-  ton.enableBluetooth();
-  ton.enableIMU();  /* Deixe o robô parado durante a inicialização (~5s) */
-
-  /* Configura o Timer da IMU - intervalo = 0.1 segundos */
-  imuTick.attach(&imuCallback, 0.1);
+  /* Inicialização do robô para seguir linha preta (pista branca) */
+  ton.setLED(GREEN);
+  initTonBot(PRETA);
 
   /* Inicio do programa ------------------------------------------------------*/
-  usb.printf("Programa TESTE - TON-BOT!\r\n");
-  ton.setLED(WHITE);
+  /* Aguarda o botão USER ser pressionado para ligar o robô */
+  while(!ton.USERisPressed())
+  {
+    ton.toggleLED(RED);
+    wait_ms(100);
+  }
   wait(1);
-  ton.setLED(NONE);
+  ton.setLED(BLUE);
   beeps(3, 100, 50);
 
-  resetEncoderEsquerda();
-  resetEncoderDireita();
 
   /* LOOP principal ----------------------------------------------------------*/
   while (1)
   {
-    /* Zera e inicia o timer para estabelecer a base de tempo */
-    t1.reset();
-    t1.start();
-
-    /* Envia mensagens de estados dos periféricos a cada 1s para a USB */
-    usb.printf("Sensores de Linha (erro): %d\r\n", getSensoresLinha());
-    usb.printf("\r\n");
-
-    usb.printf("Sensores de Paredes: %d\r\n", getSensoresParede(&lf, &l, &r, &rf));
-    usb.printf("%0.3f | %0.3f | %0.3f | %0.3f\r\n", lf, l, r, rf);
-    usb.printf("\r\n");
-
-    usb.printf("IMU - Algoritmo AHRS:\r\n");
-    usb.printf("pitch: %0.3f\r\n", imu.getPitch());
-    usb.printf("roll: %0.3f\r\n", imu.getRoll());
-    usb.printf("yaw: %0.3f\r\n", imu.getYaw());
-    usb.printf("\r\n");
-
-    usb.printf("Bateria: %0.3fV\r\n", ton.getBattery());
-    usb.printf("\r\n");
-
-    usb.printf("Encoders: %d | %d\r\n", getEncoderEsquerda(), getEncoderDireita());
-    usb.printf("\r\n");
-
-    usb.printf("---\r\n");
-
-
-    /* Caso queira direcionar alguma mensagem para o Bluetooth, basta..
-     * mudar de 'usb.' para 'bluetooth.' - como o exemplo a seguir: */
-    bluetooth.printf("%0.3f | %0.3f | %0.3f | %0.3f\r\n", lf, l, r, rf);
-
-
-    /* Se o botão for pressionado: liga os motores por 1 segundo
-     * .. ou aguarda 1 segundo sem fazer nada */
-    while (t1.read_ms() < 1000)
+    /* Verifica a leitura dos sensores */
+    erro = getSensoresLinha();
+    if (erro == INFINITO)
     {
-      if (ton.USERisPressed())
-      {
-        ton.setLED(YELLOW);
-        beep(100);
-        setMotores(0.2, -0.2); /* CUIDADO COM VELOCIDADES ALTAS (~ 2.0 m/s) */
-        wait(1);
-        setMotores(0, 0);
-        ton.setLED(NONE);
-      }
+      erro = erro_anterior;
     }
 
+    /* Controlador Proporcional */
+    MV = KP * erro;
 
-    /* Pisca o LED verde */
-    ton.toggleLED(GREEN);
+    /* Acionamento dos motores */
+    setMotores(PWM_MEDIO + MV, PWM_MEDIO - MV);
+
+    /* Base de tempo do controlador */
+    wait_ms(10);
 
   } // fim do loop principal
 } // fim da função principal