IOTON Technology / ton-bot_seguidor_linha

Programa de exemplo do Robô TON-BOT no modo seguidor de linha.

Dependencies:   IOTON-API QEI USBDevice mbed-ton ton-bot

Fork of ton-bot_teste by IOTON Technology

main.cpp@1:584ac95c4061, 2017-06-29 (annotated)

Committer:
krebyy
Date:
Thu Jun 29 20:59:25 2017 +0000
Revision:
1:584ac95c4061
Parent:
0:52cc151dc505
Child:
2:027fe033f5f7
Commit inicial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krebyy 1:584ac95c4061 1 /**
krebyy 1:584ac95c4061 2 ******************************************************************************
krebyy 1:584ac95c4061 3 * @file main.cpp
krebyy 1:584ac95c4061 4 * @author Kleber Lima da Silva (kleber@ioton.cc)
krebyy 1:584ac95c4061 5 * @version V0.0.1
krebyy 1:584ac95c4061 6 * @date 19-Junho-2017
krebyy 1:584ac95c4061 7 * @brief Programa para testes do hardware do Robô TON-BOT.
krebyy 1:584ac95c4061 8 ******************************************************************************
krebyy 1:584ac95c4061 9 * @attention
krebyy 1:584ac95c4061 10 *
krebyy 1:584ac95c4061 11 * COPYRIGHT(c) 2017 IOTON Technology
krebyy 1:584ac95c4061 12 *
krebyy 1:584ac95c4061 13 * Licensed under the Apache License, Version 2.0 (the "License");
krebyy 1:584ac95c4061 14 * you may not use this file except in compliance with the License.
krebyy 1:584ac95c4061 15 * You may obtain a copy of the License at
krebyy 1:584ac95c4061 16 *
krebyy 1:584ac95c4061 17 * http://www.apache.org/licenses/LICENSE-2.0
krebyy 1:584ac95c4061 18 *
krebyy 1:584ac95c4061 19 * Unless required by applicable law or agreed to in writing, software
krebyy 1:584ac95c4061 20 * distributed under the License is distributed on an "AS IS" BASIS,
krebyy 1:584ac95c4061 21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
krebyy 1:584ac95c4061 22 * See the License for the specific language governing permissions and
krebyy 1:584ac95c4061 23 * limitations under the License.
krebyy 1:584ac95c4061 24 ******************************************************************************
krebyy 1:584ac95c4061 25 */
krebyy 0:52cc151dc505 26
krebyy 1:584ac95c4061 27 /* Includes ------------------------------------------------------------------*/
krebyy 1:584ac95c4061 28 #include "main.h"
krebyy 0:52cc151dc505 29
krebyy 1:584ac95c4061 30 /** @addtogroup Teste TON-BOT
krebyy 1:584ac95c4061 31 * @{
krebyy 1:584ac95c4061 32 */
krebyy 0:52cc151dc505 33
krebyy 1:584ac95c4061 34 /* Variáveis privadas --------------------------------------------------------*/
krebyy 1:584ac95c4061 35 Ticker imuTick; /* Timer para o algoritmo da IMU */
krebyy 1:584ac95c4061 36 Timer t1; /* Timer auxiliar */
krebyy 0:52cc151dc505 37
krebyy 0:52cc151dc505 38
krebyy 1:584ac95c4061 39 /* Funções Privadas ----------------------------------------------------------*/
krebyy 1:584ac95c4061 40
krebyy 1:584ac95c4061 41 /**
krebyy 1:584ac95c4061 42 * @brief Callback para execução do Algoritmo AHRS com taxa de 10 Hz
krebyy 1:584ac95c4061 43 * @note O algoritmo AHRS calcula os ângulos: roll, pitch and yaw
krebyy 1:584ac95c4061 44 * @param Nenhum
krebyy 1:584ac95c4061 45 * @retval Nenhum
krebyy 1:584ac95c4061 46 */
krebyy 1:584ac95c4061 47 void imuCallback(void)
krebyy 1:584ac95c4061 48 {
krebyy 1:584ac95c4061 49 // imu.runAHRS(0.1); // ### FIX: TICKER com bug nesta versao do mbed
krebyy 1:584ac95c4061 50 } // fim da imuCallback
krebyy 1:584ac95c4061 51
krebyy 1:584ac95c4061 52 /**
krebyy 1:584ac95c4061 53 * @brief Programa Principal
krebyy 1:584ac95c4061 54 * @param Nenhum
krebyy 1:584ac95c4061 55 * @retval Nenhum
krebyy 1:584ac95c4061 56 */
krebyy 1:584ac95c4061 57 int main(void)
krebyy 0:52cc151dc505 58 {
krebyy 1:584ac95c4061 59 float lf = 0, l = 0, r = 0, rf = 0;
krebyy 1:584ac95c4061 60
krebyy 1:584ac95c4061 61 /* Ligue o TON com o botão USER pressionado para indicar o estado da bateria */
krebyy 1:584ac95c4061 62 if (ton.USERisPressed())
krebyy 1:584ac95c4061 63 {
krebyy 1:584ac95c4061 64 /* Necessário pressionar o botão RESET voltar ao modo normal */
krebyy 1:584ac95c4061 65 ton.batteryStatus();
krebyy 1:584ac95c4061 66 }
krebyy 1:584ac95c4061 67
krebyy 1:584ac95c4061 68 /* Inicialização dos periféricos */
krebyy 1:584ac95c4061 69 ton.setLED(BLUE);
krebyy 1:584ac95c4061 70 initTonBot();
krebyy 1:584ac95c4061 71 ton.setLED(RED);
krebyy 1:584ac95c4061 72 ton.enableBluetooth();
krebyy 1:584ac95c4061 73 ton.enableIMU(); /* Deixe o robô parado durante a inicialização (~5s) */
krebyy 1:584ac95c4061 74
krebyy 1:584ac95c4061 75 /* Configura o Timer da IMU - intervalo = 0.1 segundos */
krebyy 1:584ac95c4061 76 imuTick.attach(&imuCallback, 0.1);
krebyy 1:584ac95c4061 77
krebyy 1:584ac95c4061 78 /* Inicio do programa ------------------------------------------------------*/
krebyy 1:584ac95c4061 79 usb.printf("Programa TESTE - TON-BOT!\r\n");
krebyy 1:584ac95c4061 80 ton.setLED(WHITE);
krebyy 1:584ac95c4061 81 wait(1);
krebyy 1:584ac95c4061 82 ton.setLED(NONE);
krebyy 1:584ac95c4061 83 beeps(3, 100, 50);
krebyy 1:584ac95c4061 84
krebyy 1:584ac95c4061 85 resetEncoderEsquerda();
krebyy 1:584ac95c4061 86 resetEncoderDireita();
krebyy 0:52cc151dc505 87
krebyy 1:584ac95c4061 88 /* LOOP principal ----------------------------------------------------------*/
krebyy 1:584ac95c4061 89 while (1)
krebyy 1:584ac95c4061 90 {
krebyy 1:584ac95c4061 91 /* Zera e inicia o timer para estabelecer a base de tempo */
krebyy 1:584ac95c4061 92 t1.reset();
krebyy 1:584ac95c4061 93 t1.start();
krebyy 1:584ac95c4061 94
krebyy 1:584ac95c4061 95 /* Envia mensagens de estados dos periféricos a cada 1s para a USB */
krebyy 1:584ac95c4061 96 usb.printf("Sensores de Linha (erro): %d\r\n", getSensoresLinha());
krebyy 1:584ac95c4061 97 usb.printf("\r\n");
krebyy 1:584ac95c4061 98
krebyy 1:584ac95c4061 99 usb.printf("Sensores de Paredes: %d\r\n", getSensoresParede(&lf, &l, &r, &rf));
krebyy 1:584ac95c4061 100 usb.printf("%0.3f | %0.3f | %0.3f | %0.3f\r\n", lf, l, r, rf);
krebyy 1:584ac95c4061 101 usb.printf("\r\n");
krebyy 1:584ac95c4061 102
krebyy 1:584ac95c4061 103 usb.printf("IMU - Algoritmo AHRS:\r\n");
krebyy 1:584ac95c4061 104 usb.printf("pitch: %0.3f\r\n", imu.getPitch());
krebyy 1:584ac95c4061 105 usb.printf("roll: %0.3f\r\n", imu.getRoll());
krebyy 1:584ac95c4061 106 usb.printf("yaw: %0.3f\r\n", imu.getYaw());
krebyy 1:584ac95c4061 107 usb.printf("\r\n");
krebyy 1:584ac95c4061 108
krebyy 1:584ac95c4061 109 usb.printf("Bateria: %0.3fV\r\n", ton.getBattery());
krebyy 1:584ac95c4061 110 usb.printf("\r\n");
krebyy 1:584ac95c4061 111
krebyy 1:584ac95c4061 112 usb.printf("Encoders: %d | %d\r\n", getEncoderEsquerda(), getEncoderDireita());
krebyy 1:584ac95c4061 113 usb.printf("\r\n");
krebyy 1:584ac95c4061 114
krebyy 1:584ac95c4061 115 usb.printf("---\r\n");
krebyy 0:52cc151dc505 116
krebyy 0:52cc151dc505 117
krebyy 1:584ac95c4061 118 /* Caso queira direcionar alguma mensagem para o Bluetooth, basta..
krebyy 1:584ac95c4061 119 * mudar de 'usb.' para 'bluetooth.' - como o exemplo a seguir: */
krebyy 1:584ac95c4061 120 bluetooth.printf("%0.3f | %0.3f | %0.3f | %0.3f\r\n", lf, l, r, rf);
krebyy 0:52cc151dc505 121
krebyy 0:52cc151dc505 122
krebyy 1:584ac95c4061 123 /* Se o botão for pressionado: liga os motores por 1 segundo
krebyy 1:584ac95c4061 124 * .. ou aguarda 1 segundo sem fazer nada */
krebyy 1:584ac95c4061 125 while (t1.read_ms() < 1000)
krebyy 0:52cc151dc505 126 {
krebyy 1:584ac95c4061 127 if (ton.USERisPressed())
krebyy 1:584ac95c4061 128 {
krebyy 1:584ac95c4061 129 ton.setLED(YELLOW);
krebyy 1:584ac95c4061 130 beep(100);
krebyy 1:584ac95c4061 131 setMotores(0.2, -0.2); /* CUIDADO COM VELOCIDADES ALTAS (~ 2.0 m/s) */
krebyy 1:584ac95c4061 132 wait(1);
krebyy 1:584ac95c4061 133 setMotores(0, 0);
krebyy 1:584ac95c4061 134 ton.setLED(NONE);
krebyy 1:584ac95c4061 135 }
krebyy 1:584ac95c4061 136 }
krebyy 0:52cc151dc505 137
krebyy 0:52cc151dc505 138
krebyy 1:584ac95c4061 139 /* Pisca o LED verde */
krebyy 1:584ac95c4061 140 ton.toggleLED(GREEN);
krebyy 0:52cc151dc505 141
krebyy 1:584ac95c4061 142 } // fim do loop principal
krebyy 1:584ac95c4061 143 } // fim da função principal
krebyy 0:52cc151dc505 144
krebyy 0:52cc151dc505 145
krebyy 1:584ac95c4061 146 /**
krebyy 1:584ac95c4061 147 * @}
krebyy 1:584ac95c4061 148 */
krebyy 0:52cc151dc505 149
krebyy 1:584ac95c4061 150 /************************ (C) COPYRIGHT IOTON Technology **********************/
krebyy 1:584ac95c4061 151 /***********************************END OF FILE********************************/