IOTON Technology / ton-bot

Biblioteca do Robô TON-BOT (https://ioton.cc/ton-bot)

Dependents:   ton-bot_teste ton-bot_seguidor_linha ton-bot_seguidor_parede

ton-bot.cpp@0:a79d499cd558, 2017-06-29 (annotated)

Committer:
krebyy
Date:
Thu Jun 29 20:53:52 2017 +0000
Revision:
0:a79d499cd558
Commit inicial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krebyy 0:a79d499cd558 1 /**
krebyy 0:a79d499cd558 2 ******************************************************************************
krebyy 0:a79d499cd558 3 * @file ton-bot.cpp
krebyy 0:a79d499cd558 4 * @author Kleber Lima da Silva (kleber@ioton.cc)
krebyy 0:a79d499cd558 5 * @version V0.0.1
krebyy 0:a79d499cd558 6 * @date 19-Junho-2017
krebyy 0:a79d499cd558 7 * @brief Biblioteca do Robô TON-BOT (https://ioton.cc/ton-bot).
krebyy 0:a79d499cd558 8 ******************************************************************************
krebyy 0:a79d499cd558 9 * @attention
krebyy 0:a79d499cd558 10 *
krebyy 0:a79d499cd558 11 * COPYRIGHT(c) 2017 IOTON Technology
krebyy 0:a79d499cd558 12 *
krebyy 0:a79d499cd558 13 * Licensed under the Apache License, Version 2.0 (the "License");
krebyy 0:a79d499cd558 14 * you may not use this file except in compliance with the License.
krebyy 0:a79d499cd558 15 * You may obtain a copy of the License at
krebyy 0:a79d499cd558 16 *
krebyy 0:a79d499cd558 17 * http://www.apache.org/licenses/LICENSE-2.0
krebyy 0:a79d499cd558 18 *
krebyy 0:a79d499cd558 19 * Unless required by applicable law or agreed to in writing, software
krebyy 0:a79d499cd558 20 * distributed under the License is distributed on an "AS IS" BASIS,
krebyy 0:a79d499cd558 21 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
krebyy 0:a79d499cd558 22 * See the License for the specific language governing permissions and
krebyy 0:a79d499cd558 23 * limitations under the License.
krebyy 0:a79d499cd558 24 ******************************************************************************
krebyy 0:a79d499cd558 25 */
krebyy 0:a79d499cd558 26
krebyy 0:a79d499cd558 27 /* Includes ------------------------------------------------------------------*/
krebyy 0:a79d499cd558 28 #include "ton-bot.h" /* Veja este arquivo com a documentação/protótipos das funções */
krebyy 0:a79d499cd558 29
krebyy 0:a79d499cd558 30
krebyy 0:a79d499cd558 31 /******************************************************************************/
krebyy 0:a79d499cd558 32 /** @addtogroup Biblioteca do Robô TON-BOT
krebyy 0:a79d499cd558 33 * @{
krebyy 0:a79d499cd558 34 */
krebyy 0:a79d499cd558 35
krebyy 0:a79d499cd558 36 /* Variáveis privadas --------------------------------------------------------*/
krebyy 0:a79d499cd558 37 PwmOut r_motor_in2(PIN0);
krebyy 0:a79d499cd558 38 PwmOut r_motor_in1(PIN1);
krebyy 0:a79d499cd558 39 PwmOut l_motor_in1(PIN2);
krebyy 0:a79d499cd558 40 PwmOut l_motor_in2(PIN3);
krebyy 0:a79d499cd558 41 DigitalOut buzzer(PIN4);
krebyy 0:a79d499cd558 42 QEI l_encoder(PIN8, PIN7, NC, 360, QEI::X4_ENCODING);
krebyy 0:a79d499cd558 43 QEI r_encoder(PIN5, PIN6, NC, 360, QEI::X4_ENCODING);
krebyy 0:a79d499cd558 44 DigitalOut lf_emitter(PIN9);
krebyy 0:a79d499cd558 45 DigitalOut side_emitter(PIN10);
krebyy 0:a79d499cd558 46 DigitalOut rf_emitter(PIN11);
krebyy 0:a79d499cd558 47 DigitalOut alternate_en(PIN12);
krebyy 0:a79d499cd558 48 DigitalOut line_emitter(PIN13);
krebyy 0:a79d499cd558 49 DigitalIn line1(PIN14);
krebyy 0:a79d499cd558 50 DigitalIn line2(PIN15);
krebyy 0:a79d499cd558 51 DigitalIn line3(PIN16);
krebyy 0:a79d499cd558 52 DigitalIn line4(PIN17);
krebyy 0:a79d499cd558 53 DigitalIn line5(PIN18);
krebyy 0:a79d499cd558 54 DigitalIn line6(PIN19);
krebyy 0:a79d499cd558 55 DigitalIn line7(PIN20);
krebyy 0:a79d499cd558 56 DigitalIn line8(PIN21);
krebyy 0:a79d499cd558 57 AnalogIn lf_receiver(PIN22);
krebyy 0:a79d499cd558 58 AnalogIn rf_receiver(PIN23);
krebyy 0:a79d499cd558 59 AnalogIn r_receiver(PIN24);
krebyy 0:a79d499cd558 60 AnalogIn l_receiver(PIN25);
krebyy 0:a79d499cd558 61
krebyy 0:a79d499cd558 62 Timeout flipper;
krebyy 0:a79d499cd558 63
krebyy 0:a79d499cd558 64 int LINHA = DEFAULT_LINHA;
krebyy 0:a79d499cd558 65 float FRONTAL_TH = DEFAULT_FRONTAL_TH;
krebyy 0:a79d499cd558 66 float LATERAL_TH = DEFAULT_LATERAL_TH;
krebyy 0:a79d499cd558 67
krebyy 0:a79d499cd558 68 void initTonBot(int cor_linha, float frontal_th, float lateral_th)
krebyy 0:a79d499cd558 69 {
krebyy 0:a79d499cd558 70 /* Configura os sensores */
krebyy 0:a79d499cd558 71 LINHA = cor_linha;
krebyy 0:a79d499cd558 72 FRONTAL_TH = frontal_th;
krebyy 0:a79d499cd558 73 LATERAL_TH = lateral_th;
krebyy 0:a79d499cd558 74
krebyy 0:a79d499cd558 75 /* Inicializa os PWMs */
krebyy 0:a79d499cd558 76 l_motor_in1.period_us(50);
krebyy 0:a79d499cd558 77 l_motor_in2.period_us(50);
krebyy 0:a79d499cd558 78 r_motor_in1.period_us(50);
krebyy 0:a79d499cd558 79 r_motor_in2.period_us(50);
krebyy 0:a79d499cd558 80
krebyy 0:a79d499cd558 81 /* Inicializa os Encoders */
krebyy 0:a79d499cd558 82 resetEncoderEsquerda();
krebyy 0:a79d499cd558 83 resetEncoderDireita();
krebyy 0:a79d499cd558 84 }
krebyy 0:a79d499cd558 85
krebyy 0:a79d499cd558 86
krebyy 0:a79d499cd558 87 /******************************************************************************/
krebyy 0:a79d499cd558 88 /** @defgroup Buzzer
krebyy 0:a79d499cd558 89 * @{
krebyy 0:a79d499cd558 90 */
krebyy 0:a79d499cd558 91
krebyy 0:a79d499cd558 92 void beeps(uint8_t vezes, uint16_t t_on, uint16_t t_off)
krebyy 0:a79d499cd558 93 {
krebyy 0:a79d499cd558 94 for (uint8_t i = 0; i < vezes; i++)
krebyy 0:a79d499cd558 95 {
krebyy 0:a79d499cd558 96 buzzer = 1;
krebyy 0:a79d499cd558 97 wait_ms(t_on);
krebyy 0:a79d499cd558 98 buzzer = 0;
krebyy 0:a79d499cd558 99 wait_ms(t_off);
krebyy 0:a79d499cd558 100 }
krebyy 0:a79d499cd558 101 }
krebyy 0:a79d499cd558 102
krebyy 0:a79d499cd558 103 void beepOff()
krebyy 0:a79d499cd558 104 {
krebyy 0:a79d499cd558 105 buzzer = 0;
krebyy 0:a79d499cd558 106 }
krebyy 0:a79d499cd558 107
krebyy 0:a79d499cd558 108 void beep(uint16_t duracao)
krebyy 0:a79d499cd558 109 {
krebyy 0:a79d499cd558 110 buzzer = 1;
krebyy 0:a79d499cd558 111 flipper.attach(&beepOff, (float)duracao/1000.0f);
krebyy 0:a79d499cd558 112 }
krebyy 0:a79d499cd558 113
krebyy 0:a79d499cd558 114 /**
krebyy 0:a79d499cd558 115 * @}
krebyy 0:a79d499cd558 116 */
krebyy 0:a79d499cd558 117
krebyy 0:a79d499cd558 118
krebyy 0:a79d499cd558 119 /******************************************************************************/
krebyy 0:a79d499cd558 120 /** @defgroup Encoders
krebyy 0:a79d499cd558 121 * @{
krebyy 0:a79d499cd558 122 */
krebyy 0:a79d499cd558 123
krebyy 0:a79d499cd558 124 int32_t getEncoderEsquerda(void)
krebyy 0:a79d499cd558 125 {
krebyy 0:a79d499cd558 126 return l_encoder.getPulses();
krebyy 0:a79d499cd558 127 }
krebyy 0:a79d499cd558 128
krebyy 0:a79d499cd558 129
krebyy 0:a79d499cd558 130 void resetEncoderEsquerda(void)
krebyy 0:a79d499cd558 131 {
krebyy 0:a79d499cd558 132 l_encoder.reset();
krebyy 0:a79d499cd558 133 }
krebyy 0:a79d499cd558 134
krebyy 0:a79d499cd558 135
krebyy 0:a79d499cd558 136 int32_t getEncoderDireita(void)
krebyy 0:a79d499cd558 137 {
krebyy 0:a79d499cd558 138 return r_encoder.getPulses();
krebyy 0:a79d499cd558 139 }
krebyy 0:a79d499cd558 140
krebyy 0:a79d499cd558 141
krebyy 0:a79d499cd558 142 void resetEncoderDireita(void)
krebyy 0:a79d499cd558 143 {
krebyy 0:a79d499cd558 144 r_encoder.reset();
krebyy 0:a79d499cd558 145 }
krebyy 0:a79d499cd558 146
krebyy 0:a79d499cd558 147 /**
krebyy 0:a79d499cd558 148 * @}
krebyy 0:a79d499cd558 149 */
krebyy 0:a79d499cd558 150
krebyy 0:a79d499cd558 151
krebyy 0:a79d499cd558 152 /******************************************************************************/
krebyy 0:a79d499cd558 153 /** @defgroup Motores
krebyy 0:a79d499cd558 154 * @{
krebyy 0:a79d499cd558 155 */
krebyy 0:a79d499cd558 156
krebyy 0:a79d499cd558 157 void setMotores(float pwm_esquerda, float pwm_direita, bool fast_decay)
krebyy 0:a79d499cd558 158 {
krebyy 0:a79d499cd558 159 /* Acionamento do motor da esquerda */
krebyy 0:a79d499cd558 160 if (pwm_esquerda < 0) /* Reverso */
krebyy 0:a79d499cd558 161 {
krebyy 0:a79d499cd558 162 pwm_esquerda *= -1;
krebyy 0:a79d499cd558 163
krebyy 0:a79d499cd558 164 /* Slow decay (default) */
krebyy 0:a79d499cd558 165 if (fast_decay == false)
krebyy 0:a79d499cd558 166 {
krebyy 0:a79d499cd558 167 l_motor_in1 = 1 - pwm_esquerda;
krebyy 0:a79d499cd558 168 l_motor_in2 = 1;
krebyy 0:a79d499cd558 169 }
krebyy 0:a79d499cd558 170 else /* Fast decay */
krebyy 0:a79d499cd558 171 {
krebyy 0:a79d499cd558 172 l_motor_in1 = 0;
krebyy 0:a79d499cd558 173 l_motor_in2 = pwm_esquerda;
krebyy 0:a79d499cd558 174 }
krebyy 0:a79d499cd558 175 }
krebyy 0:a79d499cd558 176 else /* Direto */
krebyy 0:a79d499cd558 177 {
krebyy 0:a79d499cd558 178 /* Slow decay (default) */
krebyy 0:a79d499cd558 179 if (fast_decay == false)
krebyy 0:a79d499cd558 180 {
krebyy 0:a79d499cd558 181 l_motor_in1 = 1;
krebyy 0:a79d499cd558 182 l_motor_in2 = 1 - pwm_esquerda;
krebyy 0:a79d499cd558 183 }
krebyy 0:a79d499cd558 184 else /* Fast decay */
krebyy 0:a79d499cd558 185 {
krebyy 0:a79d499cd558 186 l_motor_in1 = pwm_esquerda;
krebyy 0:a79d499cd558 187 l_motor_in2 = 0;
krebyy 0:a79d499cd558 188 }
krebyy 0:a79d499cd558 189 }
krebyy 0:a79d499cd558 190
krebyy 0:a79d499cd558 191 /* Acionamento do motor da direita */
krebyy 0:a79d499cd558 192 if (pwm_direita < 0) /* Reverso */
krebyy 0:a79d499cd558 193 {
krebyy 0:a79d499cd558 194 pwm_direita *= -1;
krebyy 0:a79d499cd558 195
krebyy 0:a79d499cd558 196 /* Slow decay (default) */
krebyy 0:a79d499cd558 197 if (fast_decay == false)
krebyy 0:a79d499cd558 198 {
krebyy 0:a79d499cd558 199 r_motor_in1 = 1 - pwm_direita;
krebyy 0:a79d499cd558 200 r_motor_in2 = 1;
krebyy 0:a79d499cd558 201 }
krebyy 0:a79d499cd558 202 else /* Fast decay */
krebyy 0:a79d499cd558 203 {
krebyy 0:a79d499cd558 204 r_motor_in1 = 0;
krebyy 0:a79d499cd558 205 r_motor_in2 = pwm_direita;
krebyy 0:a79d499cd558 206 }
krebyy 0:a79d499cd558 207 }
krebyy 0:a79d499cd558 208 else /* Direto */
krebyy 0:a79d499cd558 209 {
krebyy 0:a79d499cd558 210 /* Slow decay (default) */
krebyy 0:a79d499cd558 211 if (fast_decay == false)
krebyy 0:a79d499cd558 212 {
krebyy 0:a79d499cd558 213 r_motor_in1 = 1;
krebyy 0:a79d499cd558 214 r_motor_in2 = 1 - pwm_direita;
krebyy 0:a79d499cd558 215 }
krebyy 0:a79d499cd558 216 else /* Fast decay */
krebyy 0:a79d499cd558 217 {
krebyy 0:a79d499cd558 218 r_motor_in1 = pwm_direita;
krebyy 0:a79d499cd558 219 r_motor_in2 = 0;
krebyy 0:a79d499cd558 220 }
krebyy 0:a79d499cd558 221 }
krebyy 0:a79d499cd558 222 }
krebyy 0:a79d499cd558 223
krebyy 0:a79d499cd558 224 /**
krebyy 0:a79d499cd558 225 * @}
krebyy 0:a79d499cd558 226 */
krebyy 0:a79d499cd558 227
krebyy 0:a79d499cd558 228
krebyy 0:a79d499cd558 229 /******************************************************************************/
krebyy 0:a79d499cd558 230 /** @defgroup Sensores
krebyy 0:a79d499cd558 231 * @{
krebyy 0:a79d499cd558 232 */
krebyy 0:a79d499cd558 233
krebyy 0:a79d499cd558 234 uint8_t getSensoresParede(float* lf, float* l, float* r, float* rf)
krebyy 0:a79d499cd558 235 {
krebyy 0:a79d499cd558 236 uint8_t paredes = 0;
krebyy 0:a79d499cd558 237
krebyy 0:a79d499cd558 238 (*lf) = lf_receiver;
krebyy 0:a79d499cd558 239 (*l) = l_receiver;
krebyy 0:a79d499cd558 240 (*r) = r_receiver;
krebyy 0:a79d499cd558 241 (*rf) = rf_receiver;
krebyy 0:a79d499cd558 242
krebyy 0:a79d499cd558 243 /* Sensor frontal esquerdo */
krebyy 0:a79d499cd558 244 lf_emitter = 1;
krebyy 0:a79d499cd558 245 wait_us(60);
krebyy 0:a79d499cd558 246 (*lf) = lf_receiver - (*lf);
krebyy 0:a79d499cd558 247 lf_emitter = 0;
krebyy 0:a79d499cd558 248 if ((*lf) < 0)
krebyy 0:a79d499cd558 249 {
krebyy 0:a79d499cd558 250 (*lf) = 0;
krebyy 0:a79d499cd558 251 }
krebyy 0:a79d499cd558 252 wait_us(80);
krebyy 0:a79d499cd558 253
krebyy 0:a79d499cd558 254 /* Sensor frontal direito */
krebyy 0:a79d499cd558 255 rf_emitter = 1;
krebyy 0:a79d499cd558 256 wait_us(60);
krebyy 0:a79d499cd558 257 (*rf) = rf_receiver - (*rf);
krebyy 0:a79d499cd558 258 rf_emitter = 0;
krebyy 0:a79d499cd558 259 if ((*rf) < 0)
krebyy 0:a79d499cd558 260 {
krebyy 0:a79d499cd558 261 (*rf) = 0;
krebyy 0:a79d499cd558 262 }
krebyy 0:a79d499cd558 263 wait_us(80);
krebyy 0:a79d499cd558 264
krebyy 0:a79d499cd558 265 /* Sensores laterais */
krebyy 0:a79d499cd558 266 side_emitter = 1;
krebyy 0:a79d499cd558 267 wait_us(60);
krebyy 0:a79d499cd558 268 (*l) = l_receiver - (*l);
krebyy 0:a79d499cd558 269 (*r) = r_receiver - (*r);
krebyy 0:a79d499cd558 270 side_emitter = 0;
krebyy 0:a79d499cd558 271 if ((*l) < 0)
krebyy 0:a79d499cd558 272 {
krebyy 0:a79d499cd558 273 (*l) = 0;
krebyy 0:a79d499cd558 274 }
krebyy 0:a79d499cd558 275 if ((*r) < 0)
krebyy 0:a79d499cd558 276 {
krebyy 0:a79d499cd558 277 (*r) = 0;
krebyy 0:a79d499cd558 278 }
krebyy 0:a79d499cd558 279
krebyy 0:a79d499cd558 280
krebyy 0:a79d499cd558 281 /* Realiza a máscara de bits */
krebyy 0:a79d499cd558 282 if ((*lf) > FRONTAL_TH || (*rf) > FRONTAL_TH)
krebyy 0:a79d499cd558 283 {
krebyy 0:a79d499cd558 284 paredes |= PAREDE_FRONTAL;
krebyy 0:a79d499cd558 285 }
krebyy 0:a79d499cd558 286
krebyy 0:a79d499cd558 287 if ((*l) > LATERAL_TH)
krebyy 0:a79d499cd558 288 {
krebyy 0:a79d499cd558 289 paredes |= PAREDE_ESQUERDA;
krebyy 0:a79d499cd558 290 }
krebyy 0:a79d499cd558 291
krebyy 0:a79d499cd558 292 if ((*r) > LATERAL_TH)
krebyy 0:a79d499cd558 293 {
krebyy 0:a79d499cd558 294 paredes |= PAREDE_DIREITA;
krebyy 0:a79d499cd558 295 }
krebyy 0:a79d499cd558 296
krebyy 0:a79d499cd558 297 return paredes;
krebyy 0:a79d499cd558 298 }
krebyy 0:a79d499cd558 299
krebyy 0:a79d499cd558 300
krebyy 0:a79d499cd558 301 int32_t getSensoresLinha()
krebyy 0:a79d499cd558 302 {
krebyy 0:a79d499cd558 303 int32_t erro = 0, soma = 0, n = 0;
krebyy 0:a79d499cd558 304
krebyy 0:a79d499cd558 305 /* Habilita os emissores */
krebyy 0:a79d499cd558 306 line_emitter = 1;
krebyy 0:a79d499cd558 307 wait_us(100);
krebyy 0:a79d499cd558 308
krebyy 0:a79d499cd558 309 /* Realiza a leitura de todos os sensores de linha, os sensores das
krebyy 0:a79d499cd558 310 extremidades pussuem peso maior, no final é realizada a média ponderada */
krebyy 0:a79d499cd558 311 if (line1 == LINHA)
krebyy 0:a79d499cd558 312 {
krebyy 0:a79d499cd558 313 soma += -40;
krebyy 0:a79d499cd558 314 n++;
krebyy 0:a79d499cd558 315 }
krebyy 0:a79d499cd558 316 if (line2 == LINHA)
krebyy 0:a79d499cd558 317 {
krebyy 0:a79d499cd558 318 soma += -30;
krebyy 0:a79d499cd558 319 n++;
krebyy 0:a79d499cd558 320 }
krebyy 0:a79d499cd558 321 if (line3 == LINHA)
krebyy 0:a79d499cd558 322 {
krebyy 0:a79d499cd558 323 soma += -20;
krebyy 0:a79d499cd558 324 n++;
krebyy 0:a79d499cd558 325 }
krebyy 0:a79d499cd558 326 if (line4 == LINHA)
krebyy 0:a79d499cd558 327 {
krebyy 0:a79d499cd558 328 soma += -10;
krebyy 0:a79d499cd558 329 n++;
krebyy 0:a79d499cd558 330 }
krebyy 0:a79d499cd558 331 if (line5 == LINHA)
krebyy 0:a79d499cd558 332 {
krebyy 0:a79d499cd558 333 soma += 10;
krebyy 0:a79d499cd558 334 n++;
krebyy 0:a79d499cd558 335 }
krebyy 0:a79d499cd558 336 if (line6 == LINHA)
krebyy 0:a79d499cd558 337 {
krebyy 0:a79d499cd558 338 soma += 20;
krebyy 0:a79d499cd558 339 n++;
krebyy 0:a79d499cd558 340 }
krebyy 0:a79d499cd558 341 if (line7 == LINHA)
krebyy 0:a79d499cd558 342 {
krebyy 0:a79d499cd558 343 soma += 30;
krebyy 0:a79d499cd558 344 n++;
krebyy 0:a79d499cd558 345 }
krebyy 0:a79d499cd558 346 if (line8 == LINHA)
krebyy 0:a79d499cd558 347 {
krebyy 0:a79d499cd558 348 soma += 40;
krebyy 0:a79d499cd558 349 n++;
krebyy 0:a79d499cd558 350 }
krebyy 0:a79d499cd558 351
krebyy 0:a79d499cd558 352 /* Desabilita os emissores */
krebyy 0:a79d499cd558 353 line_emitter = 0;
krebyy 0:a79d499cd558 354
krebyy 0:a79d499cd558 355
krebyy 0:a79d499cd558 356 /* Retorna a média ou retorna a constante INFINITO indicando
krebyy 0:a79d499cd558 357 que nenhum sensor leu linha */
krebyy 0:a79d499cd558 358 if (n != 0)
krebyy 0:a79d499cd558 359 {
krebyy 0:a79d499cd558 360 erro = soma / n;
krebyy 0:a79d499cd558 361 }
krebyy 0:a79d499cd558 362 else
krebyy 0:a79d499cd558 363 {
krebyy 0:a79d499cd558 364 erro = INFINITO;
krebyy 0:a79d499cd558 365 }
krebyy 0:a79d499cd558 366
krebyy 0:a79d499cd558 367 return erro;
krebyy 0:a79d499cd558 368 }
krebyy 0:a79d499cd558 369
krebyy 0:a79d499cd558 370 /**
krebyy 0:a79d499cd558 371 * @}
krebyy 0:a79d499cd558 372 */
krebyy 0:a79d499cd558 373
krebyy 0:a79d499cd558 374
krebyy 0:a79d499cd558 375 /************************ (C) COPYRIGHT IOTON Technology **********************/
krebyy 0:a79d499cd558 376 /***********************************END OF FILE********************************/