RioBotz / MotorsLF

Header dos motores do seguidor de linha

motors.cpp@2:a68b0082faac, 2020-09-19 (annotated)

Committer:
lucalm
Date:
Sat Sep 19 18:42:59 2020 +0000
Revision:
2:a68b0082faac
Parent:
1:c32f37416338 
Header dos motores

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lucalm 0:8c5a0e6a171b 1 #include "mbed.h"
lucalm 0:8c5a0e6a171b 2 #include "motors.h"
lucalm 2:a68b0082faac 3 //#include "PID.h"
lucalm 0:8c5a0e6a171b 4
lucalm 0:8c5a0e6a171b 5 /*=================================================
lucalm 0:8c5a0e6a171b 6 --------------- CODIGO DOS MOTORES ----------------
lucalm 0:8c5a0e6a171b 7 =================================================*/
lucalm 0:8c5a0e6a171b 8
lucalm 0:8c5a0e6a171b 9 //------------------ FUNCOES ----------------------
lucalm 0:8c5a0e6a171b 10 #define velocidadeBase 0.18
lucalm 0:8c5a0e6a171b 11
lucalm 0:8c5a0e6a171b 12 PwmOut rodaEsquerda(p25); // Potencias da roda e direções
lucalm 0:8c5a0e6a171b 13 DigitalOut direcaoEsquerda (p26);
lucalm 0:8c5a0e6a171b 14 PwmOut rodaDireita(p24);
lucalm 0:8c5a0e6a171b 15 DigitalOut direcaoDireita (p23);
lucalm 0:8c5a0e6a171b 16 AnalogIn batMeasure (p22);
lucalm 0:8c5a0e6a171b 17
lucalm 0:8c5a0e6a171b 18
lucalm 2:a68b0082faac 19 void Setup_Motores(void)//Funcao de setup dos motores, ajustando o periodo para inicializar o pwm
lucalm 0:8c5a0e6a171b 20 {
lucalm 0:8c5a0e6a171b 21 rodaEsquerda.period(0.01);
lucalm 0:8c5a0e6a171b 22 rodaDireita.period(0.01);
lucalm 0:8c5a0e6a171b 23 }
lucalm 0:8c5a0e6a171b 24
lucalm 1:c32f37416338 25 void ControleMotores (float VelocidadeEsquerda, float VelocidadeDireita ){ //Função drive, controla a velocidade de cada roda
lucalm 0:8c5a0e6a171b 26
lucalm 2:a68b0082faac 27 if (VelocidadeEsquerda <= 0){
lucalm 0:8c5a0e6a171b 28 VelocidadeEsquerda = abs(VelocidadeEsquerda);//Utiliza o modulo da velocidade, pois esta nao pode ser negativa
lucalm 0:8c5a0e6a171b 29 direcaoEsquerda = 0;//Muda a direcao da roda.
lucalm 0:8c5a0e6a171b 30 rodaEsquerda.write(VelocidadeEsquerda);
lucalm 0:8c5a0e6a171b 31 }
lucalm 0:8c5a0e6a171b 32 else { direcaoEsquerda = 1; rodaEsquerda.write(VelocidadeEsquerda); }//Mantem a direcao da roda.
lucalm 0:8c5a0e6a171b 33
lucalm 0:8c5a0e6a171b 34 if (VelocidadeDireita <= 0){
lucalm 0:8c5a0e6a171b 35 VelocidadeDireita = abs(VelocidadeDireita);
lucalm 0:8c5a0e6a171b 36 direcaoDireita = 0;
lucalm 0:8c5a0e6a171b 37 rodaDireita.write(VelocidadeDireita);
lucalm 0:8c5a0e6a171b 38 }
lucalm 0:8c5a0e6a171b 39 else { direcaoDireita = 1; rodaDireita.write(VelocidadeDireita); }
lucalm 0:8c5a0e6a171b 40 }
lucalm 0:8c5a0e6a171b 41
lucalm 0:8c5a0e6a171b 42 void Acelerando(float velocidadeMax)//Acelera o seguidor de linha
lucalm 0:8c5a0e6a171b 43 {
lucalm 0:8c5a0e6a171b 44 int bandeira = 0;
lucalm 0:8c5a0e6a171b 45 float novaVelocidade = velocidadeBase;
lucalm 0:8c5a0e6a171b 46 while(bandeira == 0)
lucalm 0:8c5a0e6a171b 47 {
lucalm 0:8c5a0e6a171b 48 novaVelocidade = novaVelocidade + 0.01;
lucalm 2:a68b0082faac 49 wait(0.001);//Deve ser testado se e necessario esse wait
lucalm 0:8c5a0e6a171b 50 if (novaVelocidade <= velocidadeMax)
lucalm 0:8c5a0e6a171b 51 {
lucalm 0:8c5a0e6a171b 52 bandeira = 1;
lucalm 0:8c5a0e6a171b 53 }
lucalm 0:8c5a0e6a171b 54 }
lucalm 0:8c5a0e6a171b 55 }
lucalm 0:8c5a0e6a171b 56
lucalm 0:8c5a0e6a171b 57 void Desacelerando(float velocidadeMin)//Desacelera o seguidor de linha
lucalm 0:8c5a0e6a171b 58 {
lucalm 0:8c5a0e6a171b 59 int bandeira = 0;
lucalm 0:8c5a0e6a171b 60 float novaVelocidade = velocidadeBase;
lucalm 0:8c5a0e6a171b 61 while(bandeira == 0)
lucalm 0:8c5a0e6a171b 62 {
lucalm 0:8c5a0e6a171b 63 novaVelocidade = novaVelocidade - 0.01;
lucalm 2:a68b0082faac 64 wait(0.001);//Deve ser testado se e necessario esse wait
lucalm 0:8c5a0e6a171b 65 if (novaVelocidade <= velocidadeMin)
lucalm 0:8c5a0e6a171b 66 {
lucalm 0:8c5a0e6a171b 67 bandeira = 1;
lucalm 0:8c5a0e6a171b 68 }
lucalm 0:8c5a0e6a171b 69 }
lucalm 0:8c5a0e6a171b 70 }
lucalm 0:8c5a0e6a171b 71
lucalm 0:8c5a0e6a171b 72 void VelocidadeBateria() // Funcao de setup da velocidade base dos motores
lucalm 0:8c5a0e6a171b 73 {
lucalm 2:a68b0082faac 74 float leituraBateria = batMeasure; //Lendo a carga da bateria em um numero de 0 a 1
lucalm 0:8c5a0e6a171b 75 float tensaoBateria = 12.6 * leituraBateria; // Carga atual da bateria em Volts
lucalm 0:8c5a0e6a171b 76 float novaVelocidade = velocidadeBase * 8.4/tensaoBateria;// Calculando a nova velocidade de acordo com a medida de carga atual da bateria
lucalm 0:8c5a0e6a171b 77 return;
lucalm 0:8c5a0e6a171b 78 }
lucalm 0:8c5a0e6a171b 79
lucalm 2:a68b0082faac 80 /*float ajustaVelocidade(float velocidadeEsperada, float velocidadeReal)
lucalm 2:a68b0082faac 81 {
lucalm 2:a68b0082faac 82 Correcao_roda=PID_roda();
lucalm 2:a68b0082faac 83 velocidadeEsperada=velocidadeReal + Correcao_roda
lucalm 2:a68b0082faac 84 return velocidadeEsperada;
lucalm 2:a68b0082faac 85 }*/
lucalm 2:a68b0082faac 86
lucalm 2:a68b0082faac 87 void ParadaFinal()//Para o seguidor de linha
lucalm 0:8c5a0e6a171b 88 {
lucalm 0:8c5a0e6a171b 89 while(1)
lucalm 0:8c5a0e6a171b 90 {
lucalm 0:8c5a0e6a171b 91 rodaEsquerda.write(0);
lucalm 0:8c5a0e6a171b 92 rodaDireita.write(0);
lucalm 0:8c5a0e6a171b 93 }
lucalm 0:8c5a0e6a171b 94 } //Função de parada do LF depois do percurso
lucalm 0:8c5a0e6a171b 95
lucalm 0:8c5a0e6a171b 96 /*void Para_Demonio (void){ direcaoEsquerda = 0; direcaoDireita = 0; rodaEsquerda.write(0); rodaDireita.write(0); } //Mantém o LF parado
lucalm 0:8c5a0e6a171b 97
lucalm 0:8c5a0e6a171b 98 void Volta_Pra_Linha (){ //Faz o LF voltar para o último lado em que leu a linha, caso tenha pertido (função não funcional, precisa alterar a velocidade)
lucalm 0:8c5a0e6a171b 99 if (Position == 0){
lucalm 0:8c5a0e6a171b 100 if (lastSensor > 0) { Esquerda_Esperada = -50; Direita_Esperada = 80; }
lucalm 0:8c5a0e6a171b 101 else { Esquerda_Esperada = 80; Direita_Esperada = -50; }
lucalm 0:8c5a0e6a171b 102 }
lucalm 0:8c5a0e6a171b 103 else { lastSensor = Error; }
lucalm 0:8c5a0e6a171b 104 }
lucalm 0:8c5a0e6a171b 105 */