RioBotz / Mbed 2 deprecated LineFollowerV2-Final

Dependencies:   mbed

Sensor/Sensor.cpp@11:8d71a1e1e947, 2019-08-09 (annotated)

Committer:
amaral99
Date:
Fri Aug 09 17:11:40 2019 +0000
Revision:
11:8d71a1e1e947
Parent:
10:e412756d0f66 
.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
amaral99 0:44f2bd8f9b56 1 //================================================================================
amaral99 0:44f2bd8f9b56 2 //========================= READ SENSORS - SENSOR.CPP =========================
amaral99 0:44f2bd8f9b56 3 //================================================================================
amaral99 0:44f2bd8f9b56 4
amaral99 0:44f2bd8f9b56 5 //***************************** INCLUDES - INICIO ******************************
amaral99 0:44f2bd8f9b56 6
amaral99 0:44f2bd8f9b56 7 #include "mbed.h"
amaral99 0:44f2bd8f9b56 8 #include "Sensor.h"
amaral99 0:44f2bd8f9b56 9
amaral99 0:44f2bd8f9b56 10 //***************************** INCLUDES - FINAL ******************************
amaral99 0:44f2bd8f9b56 11 //################################################################################
amaral99 0:44f2bd8f9b56 12 //################################################################################
amaral99 0:44f2bd8f9b56 13 //**************************** DEFINICOES - INICIO *****************************
amaral99 0:44f2bd8f9b56 14
amaral99 0:44f2bd8f9b56 15 //Definicao da porta de cada sensor
amaral99 0:44f2bd8f9b56 16 //| s1 | s2 | s3 | s4 | s5 | s6 | s7 | s8 |
Raygrou 3:a0c3e850f70d 17 /*InterruptIn s1(p5);
amaral99 0:44f2bd8f9b56 18 InterruptIn s2(p6);
amaral99 0:44f2bd8f9b56 19 InterruptIn s3(p7);
amaral99 0:44f2bd8f9b56 20 InterruptIn s4(p8);
amaral99 0:44f2bd8f9b56 21 InterruptIn s5(p9);
amaral99 0:44f2bd8f9b56 22 InterruptIn s6(p12);
amaral99 0:44f2bd8f9b56 23 InterruptIn s7(p13);
Raygrou 3:a0c3e850f70d 24 InterruptIn s8(p14);*/
Raygrou 3:a0c3e850f70d 25
Raygrou 3:a0c3e850f70d 26 DigitalIn s1(p5);
Raygrou 3:a0c3e850f70d 27 DigitalIn s2(p6);
Raygrou 3:a0c3e850f70d 28 DigitalIn s3(p7);
Raygrou 3:a0c3e850f70d 29 DigitalIn s4(p8);
Raygrou 3:a0c3e850f70d 30 DigitalIn s5(p9);
Raygrou 3:a0c3e850f70d 31 DigitalIn s6(p12);
Raygrou 3:a0c3e850f70d 32 DigitalIn s7(p13);
Raygrou 3:a0c3e850f70d 33 DigitalIn s8(p14);
Raygrou 10:e412756d0f66 34 //DigitalIn sR(p30);
amaral99 0:44f2bd8f9b56 35
amaral99 0:44f2bd8f9b56 36 #define LINHA 1 //LINHA = 1 -> Linha branca e fundo preto
amaral99 0:44f2bd8f9b56 37 //LINHA = 0 -> Linha preta e fundo branco
amaral99 0:44f2bd8f9b56 38
Raygrou 10:e412756d0f66 39 int Position = 0; //Variavel que armazenara a ponsicao dos sensores
Raygrou 10:e412756d0f66 40 int PRETO = 0; //Variavel que indica se perdeu a linha
Raygrou 10:e412756d0f66 41 int lastRead = 1; //Variavel que indica ultima leitura do sensor lateral direito
Raygrou 10:e412756d0f66 42 int atualRead = 1; //Variavel que indica leitura atual do sensor lateral direito
Raygrou 6:4a772df7baba 43
amaral99 0:44f2bd8f9b56 44 //***************************** DEFINICOES - FINAL *****************************
amaral99 0:44f2bd8f9b56 45 //################################################################################
amaral99 0:44f2bd8f9b56 46 //################################################################################
amaral99 0:44f2bd8f9b56 47 //******************************* FUNCOES - INICIO ******************************
amaral99 0:44f2bd8f9b56 48
amaral99 0:44f2bd8f9b56 49 //Funcao que inicializa os sensores
amaral99 1:16f1584cf0a6 50 //Define o mbed como um resistor de PullUp
amaral99 0:44f2bd8f9b56 51 void Sensors_Setup(void) {
amaral99 0:44f2bd8f9b56 52 s1.mode(PullUp);
amaral99 0:44f2bd8f9b56 53 s2.mode(PullUp);
amaral99 0:44f2bd8f9b56 54 s3.mode(PullUp);
amaral99 0:44f2bd8f9b56 55 s4.mode(PullUp);
amaral99 0:44f2bd8f9b56 56 s5.mode(PullUp);
amaral99 0:44f2bd8f9b56 57 s6.mode(PullUp);
amaral99 0:44f2bd8f9b56 58 s7.mode(PullUp);
amaral99 0:44f2bd8f9b56 59 s8.mode(PullUp);
Raygrou 10:e412756d0f66 60 //sR.mode(PullUp);
amaral99 0:44f2bd8f9b56 61 }
amaral99 0:44f2bd8f9b56 62
Raygrou 10:e412756d0f66 63 //Funcao que checa as linhas de inicio, fim e cruzamentos
Raygrou 10:e412756d0f66 64 //So ira retornar true quando sair da linha branca para a pista preta novamente
Raygrou 10:e412756d0f66 65 /*bool Check_Final(void){
Raygrou 8:2b0c6a2aef4a 66 atualRead = sR.read();
Raygrou 8:2b0c6a2aef4a 67 if(lastRead == 0 && atualRead != lastRead) {lastRead = atualRead; return true; }
Raygrou 8:2b0c6a2aef4a 68 else{ lastRead = atualRead; return false; }
Raygrou 10:e412756d0f66 69 }*/
Raygrou 8:2b0c6a2aef4a 70
amaral99 0:44f2bd8f9b56 71 //Funcao que realiza a leitura de cada sensor
amaral99 0:44f2bd8f9b56 72 void Read_Sensors(void) {
amaral99 0:44f2bd8f9b56 73
amaral99 0:44f2bd8f9b56 74 //| s1 | s2 | s3 | s4 | s5 | s6 | s7 | s8 |
amaral99 0:44f2bd8f9b56 75 //| 10 | 20 | 30 | 40 | 50 | 60 | 70 | 80 |
amaral99 0:44f2bd8f9b56 76
amaral99 0:44f2bd8f9b56 77 int Leitura = 0, Num_Sensors; Position = 0;
amaral99 0:44f2bd8f9b56 78
amaral99 0:44f2bd8f9b56 79 if (LINHA) {
amaral99 0:44f2bd8f9b56 80 Num_Sensors = 8;
amaral99 0:44f2bd8f9b56 81
amaral99 0:44f2bd8f9b56 82 Leitura = s1.read(); Position += Leitura*10; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 83 Leitura = s2.read(); Position += Leitura*20; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 84 Leitura = s3.read(); Position += Leitura*30; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 85 Leitura = s4.read(); Position += Leitura*40; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 86 Leitura = s5.read(); Position += Leitura*50; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 87 Leitura = s6.read(); Position += Leitura*60; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 88 Leitura = s7.read(); Position += Leitura*70; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 89 Leitura = s8.read(); Position += Leitura*80; Num_Sensors -= Leitura;
amaral99 0:44f2bd8f9b56 90
Raygrou 3:a0c3e850f70d 91 if(Position == 360) { PRETO = 1; }
Raygrou 3:a0c3e850f70d 92 else { PRETO = 0; }
Raygrou 3:a0c3e850f70d 93
amaral99 0:44f2bd8f9b56 94 Position = (360 - Position) / Num_Sensors;
amaral99 0:44f2bd8f9b56 95 }
amaral99 0:44f2bd8f9b56 96
amaral99 0:44f2bd8f9b56 97 else {
amaral99 0:44f2bd8f9b56 98 Num_Sensors = 0;
amaral99 0:44f2bd8f9b56 99
amaral99 0:44f2bd8f9b56 100 Leitura = s1.read(); Position += Leitura*10; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 101 Leitura = s2.read(); Position += Leitura*20; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 102 Leitura = s3.read(); Position += Leitura*30; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 103 Leitura = s4.read(); Position += Leitura*40; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 104 Leitura = s5.read(); Position += Leitura*50; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 105 Leitura = s6.read(); Position += Leitura*60; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 106 Leitura = s7.read(); Position += Leitura*70; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 107 Leitura = s8.read(); Position += Leitura*80; Num_Sensors += Leitura;
amaral99 0:44f2bd8f9b56 108
Raygrou 4:beaad00c1396 109 if(Position == 0) { PRETO = 1; }
Raygrou 3:a0c3e850f70d 110 else { PRETO = 0; }
Raygrou 3:a0c3e850f70d 111
Raygrou 10:e412756d0f66 112 Position = Position / Num_Sensors;
amaral99 0:44f2bd8f9b56 113 }
amaral99 0:44f2bd8f9b56 114 }
amaral99 0:44f2bd8f9b56 115
amaral99 0:44f2bd8f9b56 116 //Funcao que retorna o valor da posicao dos sensores
amaral99 0:44f2bd8f9b56 117 int Get_Position(void) { return Position; }
amaral99 0:44f2bd8f9b56 118
Raygrou 10:e412756d0f66 119 //Funcao que retorna se os sensores frontais perderam a linha
Raygrou 3:a0c3e850f70d 120 int Get_Preto(void) { return PRETO; }
Raygrou 3:a0c3e850f70d 121
amaral99 0:44f2bd8f9b56 122 //******************************** FUNCOES - FINAL ******************************