projeto de robô seguidor de linha com controlador pid
DESCRIPTION
Projeto de Robô Seguidor de Linha com Controlador PIDTRANSCRIPT
Universidade Federal da Bahia
Escola Politecnica
Departamento de Engenharia Eletrica
Curso de Engenharia Eletrica
ENGC35 - Modelagem e Analise de Sistemas Dinamicos
Projeto de um Robo Seguidor de Linhacom Controlador PID
Ana Tereza BorbaFranklin LimaLucas Santana
Salvador - Bahia
Novembro de 2015
Projeto de um Robo Seguidor de Linhacom Controlador PID
Ana Tereza Borba
Franklin Lima
Lucas Santana
Trabalho apresentado ao Professor Humberto
Xavier como requisito avaliativo da disciplina
Modelagem e Analise de Sistemas Dinamicos do
curso de Engenharia Eletrica.
Orientador: Prof. Humberto Xavier.
Salvador - Bahia
Novembro de 2015
“A Matematica nao mente. Mente
quem faz mau uso dela”.
Albert Einstein
Resumo
Este projeto tem como finalidade apresentar o estado da arte da criacao de um
prototipo de um robo seguidor de linha para aplicacao dos conceitos de elaboracao de
modelos e determinacao de parametros de controle estudados no curso Modelagem de Sis-
temas Dinamicos ministrada pelo professor Humberto Xavier. Inicialmente apresentam-se
as etapas do projeto, descricao dos modelos utilizados e sao apresentadas tambem as des-
cricoes dos projetos de modo conceitual e posteriormente detalhado.
Sumario
Introducao 1
1 Projeto de Engenharia 3
1.1 Cronograma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.2 Orcamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.3 Work Breakdown Structure . . . . . . . . . . . . . . . . . . . . . . . . . . 5
1.4 RACI Chart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Modelo do Sistema 7
2.1 Modelo Cinematico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2.2 Modelo do Motor DC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.2.1 Identificacao dos Parametros . . . . . . . . . . . . . . . . . . . . . . 9
2.3 Modelo dos Sensores . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3 Projeto do Controlador 12
3.1 Projetos Conceitual e Basico . . . . . . . . . . . . . . . . . . . . . . . . . . 13
3.2 Projeto Detalhado . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2.1 Parte Mecanica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2.2 Parte Eletrica . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
4 Resultados e Conclusoes 18
Referencias Bibliograficas 20
APPENDICES 21
A Prototipo Desenvolvido 21
B Codigo PID do Arduino 23
Introducao
O controle de sistemas tem sido um objeto de estudos cada vez maior da area de en-
genharia. O ser humano tem buscado revolucionar os equipamentos e maquinas de modo
a aperfeicoar o seu funcionamento, tornando-os mais estaveis e suavizando erros. E nesse
contexto que as definicoes de modelagem de sistemas e controle aparecem como protago-
nistas em diversos cursos: Engenharia Eletrica, Eletronica, Mecanica e, principalmente,
Engenharia de Automacao e Controle.
Estudar controladores significa estudar formas de como realizar um processo, en-
tende-lo, elencar ferramentas e parametros para obter o melhor resultado.
Problema proposto
A Figura 1 mostra um veıculo autonomo que deve percorrer uma trajetoria cur-
vilınea. O problema resume-se em manter o carrinho o mais estavel possıvel ao realizar
as curvas da trilha atraves de um sistema de controle que leia a entrada da posicao do
veıculo e retorne uma resposta de alteracao da velocidade dos motores para que o prototipo
retorne ao set point estabelecido.
Sensor de
luminosidade
Veículo
Figura 1: Vista superior do robo sobre a trajetoria
A organizacao do trabalho e a que segue: no Capıtulo 1 sao discutidas as etapas do
projeto, incluindo cronograma, orcamento e distribuicao de atividades entre os integrantes
1
2
do grupo. Seguindo a sequencia, no Capıtulo 2, apresenta-se a modelagem do sistema,
com o detalhamento do modelo cinematico, do motor DC e dos sensores utilizados. O
projeto do controlador pode ser visto no Capıtulo 3 e, por fim, os resultados e conclusoes
sao apesentados no Capıtulo 4.
Capıtulo 1
Projeto de Engenharia
Neste capıtulo serao descritos topicos relativos a engenharia do projeto. Na Secao
1.1 e apresentado o cronograma do projeto, que teve duracao total de 7 semanas, enquanto
na Secao 1.2 e apresentado o orcamento parcial do projeto, omitindo o trabalho laborial,
custo operacional e a utilizacao dos recursos da Universidade. No fim do capıtulo sao
apresentados o WBS e RACI Chart.
1.1 Cronograma
O desenvolvimento deste trabalho seguiu o cronograma que pode ser visto na Ta-
bela 1.1. O inıcio das atividades foi dado como 12/10/2015.
Tabela 1.1: Cronograma das atividades
SemanaEtapa
1 2 3 4 5 6 7
Revisao bibliografica • • •Elaboracao do projeto conceitual •Elaboracao do projeto basico • •Busca de facilitadores e recursos • •Aquisicao de materiais •Identificacao dos parametros do motor •Identificacao do modelo do sensor •Modelagem do sistema • •Testes e Correcoes •Escrita do relatorio •Apresentacao •
3
4
1.2 Orcamento
A Tabela 1.2 mostra os custos na construcao do prototipo, desconsiderando os cus-
tos operacionais e a utilizacao dos recursos da Universidade. Os custos foram embolsados
pelos estudantes.
Tabela 1.2: Orcamento para o projeto
Item Descricao Qtde. Preco Unitario Preco Total
1 Arduino Leonardo 1 R$ 43,99 R$ 43,99
2 Shield L293d Driver Ponte H 1 R$ 22,90 R$ 22,90
3 Chassi + Motor com reducao 1 R$ 46,00 R$ 46,00
5 Sensor CNY70 2 R$ 4,00 R$ 8,00
6 Trimpot 10kΩ 2 R$ 2,50 R$ 5,00
7 Resistor 10kΩ 2 R$ 0,20 R$ 0,40
8 Resistor 100Ω 2 R$ 0,20 R$ 0,40
9 Barra de pinos 2 R$ 2,00 R$ 4,00
10 Jumper 3 R$ 0,30 R$ 0,90
11 Placa de Fenolite 10x10cm 1 R$ 7,75 R$ 7,75
12 Bateria 9V 2 R$ 3,35 R$ 6,70
Total R$ 146,04
5
1.3 Work Breakdown Structure
O diagrama WBS do projeto pode ser visto na Figura 1.1
Robô Seguidor de Trilha com Controlador PID
Do
cu
me
nta
çã
o
Motor DC Sensores Trilha
Modelagem Código
µC - Arduino Chassi
Identificação dos
ParâmetrosPlaca de Interface
Figura 1.1: Diagrama WBS para o projeto
6
1.4 RACI Chart
A matriz de atribuicao de responsabilidade e atividades e mostrada na Tabela 1.3.
Tabela 1.3: RACI Chart para o projeto
Ana
Ter
eza
Fra
nklin
Lim
a
Luca
sSan
tana
Hum
ber
to
Tarefas
1 Revisao bibliografica R R R
2 Elaboracao do projeto conceitual e basico R R R
3 Elaboracao do projeto detalhado R R, A R
4 Busca de facilitadores e recursos R R R C
5 Aquisicao de materiais R
6 Identificacao dos parametros do motor R, A R
7 Identificacao do modelo do sensor R R, A
8 Modelagem do sistema R R R C, I
9 Testes e Correcoes R R R
10 Escrita da documentacao R R, A R I
11 Apresentacao R R R I
Legenda
R Responsavel pela execucao da tarefa. Podem ser uma ou mais pes-
soas designadas a executar a tarefa.
A Prestador de contas. Havera somente uma pessoa designada para
esse papel.
C Consultor da tarefa. Sao pessoas com maior “know how” sobre
determinados assuntos, responsaveis por fornecerem informacoes
uteis para a conclusao da tarefa. A comunicacao com esse grupo
sera de duas vias.
I Pessoas informadas sobre o progresso e status da tarefa. A comu-
nicacao com esse grupo sera de mao unica
Capıtulo 2
Modelo do Sistema
Neste capıtulo serao descritos os modelos do sistema. Na Secao 2.1 sera descrito o
modelo cinematico, mostrando a relacao entre as dimensoes fısicas da trilha e do prototipo
e o seu comportamento dinamico, enquanto na Secao 2.2 e mostrado o modelo do mo-
tor e o procedimento experimental para determinacao dos coeficientes de sua funcao de
transferencia. Por fim, na Secao 2.3, e apresentado o modelo para os sensores.
2.1 Modelo Cinematico
Tem-se um veıculo dotado de duas rodas com tracao obtida por dois motores de
corrente contınua e uma roda omnidirecional, que com auxılio de sensores opticos que
fazem leitura da diferenca de cor da trilha, deve manter-se na trajetoria especificada no
plano, como ilustra a Figura 2.1.
O vetor em vermelho indica o gradiente da curva no ponto especificado, o vetor
em azul indica a direcao e sentido do veıculo. Para manter o veıculo na curva e preciso
que ϕ→ 0. ϕ e, portanto, a variavel controlada.
O objetivo do controlador e, deste modo, e garantir, com bom desempenho, que
29°
ϕ
Motor Esquerdo
Motor Direito
ââL
âR
Figura 2.1: Vista superior do robo sobre a trajetoria
7
8
ϕ = 0.
No entanto, nao ha o controle direto sobre o angulo ϕ, mas sim sobre a grandeza que
o determina no ponto considerado, levando em consideracao que o vetor a e a combinacao
dos vetores aL e aR (indicados na figura). Tais vetores sempre terao a mesma direcao de
a, porem, se aL e aR:
• tiverem mesmo sentido e mesmo modulo, o carro anda em linha reta;
• tiverem mesmo sentido mas modulos diferentes, o carro curva (aL ¿ aR: a direita,
aL ¡ aR: a esquerda) com deslocamento linear;
• tiverem sentidos diferentes, o carro curva sem deslocamento linear (curva em relacao
ao ponto central do eixo dos motores).
O sentido dos vetores aL e aR e analogo ao sentido de rotacao do motor e o modulo esta
associado a velocidade de rotacao.
O controle da velocidade e sentido de rotacao e feito baseado na tensao aplicada
no motor, entao nossa variavel manipulada e a tensao do motor.
De modo a compreender o comportamento dinamico do motor e preciso obter a
sua funcao de transferencia. O procedimento e descrito a seguir.
2.2 Modelo do Motor DC
A funcao de transferencia para um motor de corrente contınua controlado pela
corrente da armadura e dada pela Eq. 2.1
G(s) =Θ(s)
Va(s)=
Km
s[Ra(Js+ b) +KbKm]=Km/(Rab+KbKm)
s(τ1s+ 1)(2.1)
onde:
Θ(s) e a posicao angular em s;
Va(s) e a tensao aplicada no motor;
Kb, Km sao as constantes;
b e o coeficiente de friccao;
J e o torque aplicado ao eixo do motor.
9
O diagrama em blocos pode ser visto na Figura 2.2.
Figura 2.2: Diagrama em blocos do motor de corrente contınua
Como nao foi possıvel encontrar as especificacoes junto ao fabricante e de modo
a obter os dados mais precisos foi necessaria realizar a identificacao dos parametros dos
motores. O processo e descrito na secao seguinte.
2.2.1 Identificacao dos Parametros
De modo a desempenhar a identificacao dos parametros do motor utilizado, foi ne-
cessario realizar experimento com auxılio de um sensor optico, com o objetivo e obter uma
resposta aproximada a resposta ao degrau do motor. O metodo utilizado esta disponıvel
em [1].
O grafico da Figura 2.3 mostra o resultado aproximado da resposta ao degrau do
motor.
Tempo (s)2 3 4 5 6 7 8 9 10
0
0.2
0.4
0.6
0.8
1
Resposta ao Degrau
Figura 2.3: Resposta ao degrau bastante ruidosa
10
Como e possıvel observar, a resposta e bastante ruidosa por diversos motivos: a
velocidade do motor varia, ha perda de pulsos na contagem oriunda do enconder, alem
de que o tempo de medicao e o perıodo de amostragem nao coincidem precisamente [1].
Com auxılio de um filtro passa-baixas (Figura 2.4), consegue-se obter uma resposta
melhor.
Figura 2.4: Filtro utilizado no processo
O grafico da Figura 2.5 mostra a resposta filtrada.
Figura 2.5: Resposta ao degrau filtrada
O arranjo experimental desenvolvido pode ser visto na Figura A.3 disponıvel no
Apendice A.
Utilizando-se a ferramenta Parameter Estimation do Matlab foi possıvel a es-
timacao dos parametros, como mostra a Figura 2.6.
11
Figura 2.6: Modelo aproximado (esq.) e parametros estimados
2.3 Modelo dos Sensores
O comportamento do sensor esta disponıvel na suas especificacoes fornecidas pelo
fabricante. A Figura 2.7 mostra a corrente eletrica que flui na malha do receptor de
acordo com o distancia do sensor para uma dada superfıcie.
0.0
0.1
0.2
0.3
0.4
0.5
0.6
0.7
0.8
0.9
1.0
0
s - Displacement (mm)96 11915
VCE = 5 VIF = 20 mA
I
- R
ela
tive
Co
llecto
r C
urr
en
tC
rel
d = 5 mm4 mm3 mm2 mm1 mm0
E D
D
E
1.5
d
s0
5 mm
10 mm
s0
5 mm
10 mm
1110987654321
Figura 2.7: Corrente eletrica relativa versus distancia
Embora o sensor responda continuamente a distancia, podemos definir limites de
modo que:
VOUT =
0, VOUT < 2V
1, cc.
Onde ‘1’ e ‘0’ representam nıveis logicos para famılia TTL.
Capıtulo 3
Projeto do Controlador
O controlador escolhido para ser utilizado no projeto e o PID (Proporcional-
Integral-Derivativo), que e o algoritmo de controle mais usado na industria e tem sido
utilizado em todo o mundo para sistemas de controle industrial. A popularidade de con-
troladores PID pode ser atribuıda em parte ao seu desempenho robusto em uma ampla
gama de condicoes de funcionamento e em parte a sua simplicidade funcional, que permite
aos engenheiros opera-los de uma forma simples e direta.
O sinal de controle u(t) para este controlador e dado por:
u(t) = Kpe(t) +Ki
∫ t
0
e(τ)dτ +Kdd
dte(t) (3.1)
onde:
Kp e o ganho proporcional;
Ki e o ganho integral;
Kd e o ganho derivativo.
O processo de determinacao dos ganhos e conhecido com sintonia e foi feito com
auxılio do MATLAB. A Figura 3.1 mostra parte do processo. Basicamente, o MATLAB,
utilizando-se do modelo linearizado da planta, gera a saıda do controlador PID que pode
ser ajustada de acordo com os criterios de tempo de resposta (rapido versus lento) e
comportamento do transiente (agressivo versus robusto).
12
13
Figura 3.1: Ferramenta para sintonia do PID
3.1 Projetos Conceitual e Basico
O projeto de um robo seguidor de linha com controlador PID consiste na construcao
de um prototipo de veıculo autonomo cujo objetivo e percorrer uma trajetoria curvilınea
com desempenho aceitavel.
De modo a controlar o movimento cinematico do robo, e preciso identificar a sua
posicao na trilha, processo no qual sao utilizados dois sensores de luminosidade na frente
do prototipo que irao diferenciar quando o carro estiver seguindo a linha preta ou quando
sair e estiver na regiao branca. O erro e, portanto, a diferenca entre o set point, relacionado
com a velocidade do motor, e por conseguinte a posicao do veıculo e a saıda do sensor,
que determina a atual posicao do veıculo na trilha. E funcao do controlador alterar a
velocidade dos motores de modo que o veıculo mantenha-se na trajetoria.
O diagrama em blocos do sistema de controle a ser projetado pode ser visto na
Figura 3.2.
Na proxima secao o projeto detalhado e apresentado.
14
-
PID Interface Ponte H
MotorDireito
MotorEsquerdo
Se
nso
res
InterfaceSensor
+
Referência
Figura 3.2: Diagrama em blocos do sistema a ser projetado
3.2 Projeto Detalhado
Nesta secao serao descritos detalhadamente as etapas projeto, apresentado os es-
quematicos e codigos desenvolvidos, bem como os dispositivos utilizados.
3.2.1 Parte Mecanica
Na parte mecanica destaca-se o sistema de tracao e o suporte o mesmo (chassi).
A Figura 3.3 mostra as rodas e motores com a caixa de reducao utilizados no projeto.
Figura 3.3: Motores com caixa de reducao e as rodas utilizadas no projeto
15
3.2.2 Parte Eletrica
Arduino e Interface com os Motores
Arduino e uma plataforma baseada no microcontrolador ATmega32u4, que permite
o desenvolvimento eficiente e rapido de diversas aplicacoes. O Arduino utilizado pode ser
visto na Figura 3.4.
Figura 3.4: Arduino Leornado (esq.) e Motor Shield utilizados no prototipo
Para o controle do sentido de rotacao dos motores, bem como o devido interfacea-
mento entre o Arduino e os motores, a placa Motor Shield usada pode ser vista na Figura
3.4 (dir.).
Sensores
O sensor escolhido para determinacao da posicao do veıculo na trilha foi o CNY70,
que pode ser visto na Figura 3.5
Figura 3.5: Sensor CNY70 utilizado no prototipo
Um esquema do circuito de interface dos sensores pode ser visto na Figura 3.6.
16
R1
100
R2
4.7k
5V
10k
R3
5V
Sensor CNY70
VOUT
Figura 3.6: Circuito de interface para o sensor
Implementacao do PID no Arduino
O controlador PID discreto foi implementado para Arduino utilizando os valores
de ganhos (KP ,KI ,KD) determinados pelo MATLAB. Trecho do codigo implementado
para o Arduino pode ser visto abaixo. O codigo completo esta disponıvel no Apendice B.
Codigo 3.1: Trecho de codigo da implementacao do PID
1 int error = position - 2500;
2
3 int motorSpeed = Kp * error + Kd * (error - lastError);
4 lastError = error;
5
6 int rightMotorSpeed = rightBaseSpeed + motorSpeed;
7 int leftMotorSpeed = leftBaseSpeed - motorSpeed;
17
Simulacao
As simulacoes do sistema foram realizadas no ambiente Simulink do MATLAB. A
Figura 3.7 mostra o diagrama montado na plataforma.
Figura 3.7: Diagrama em blocos do sistema projetado em Simulink
Capıtulo 4
Resultados e Conclusoes
O grafico abaixo mostra em rosa a velocidade relativa, enquanto azul e amarelo
representam os motores. Os PIDs estao sintonizados diferentemente de maneira a mostrar
como o erro varia e portanto como o carro segue a curva.
Figura 4.1: Velocidade dos motores e velocidade relativa
Num teste mais realista, simulando o sinal emitido pelos sensores, obtem-se o
grafico da Figura 4.2
18
19
Figura 4.2: Velocidade dos motores e velocidade relativa para sinal mais realista
O trabalho foi extremamente importante para colocar em pratica os conceitos dados
na sala de aula sobre modelagem de sistemas fısicos, controlador PID, realimentacao, erro
e modelagem por parametros de curva S. Antes de o controle PID ter sido o objeto de
estudo e realizacao, o robo funcionava de modo ON-OFF, em que os sensores apenas
sinalizavam para que os motores ligassem caso fosse detectada a superfıcie branca. Como
resultado a trajetoria do robo era muito instavel e com muitas variacoes, o que fazia o
erro ser muito alto. Com o controlador PID foi possıvel suavizar a trajetoria do robo
consideravelmente para que ele seguisse a trilha de forma aperfeicoada, diminuindo o erro
do sistema na trajetoria. O Arduıno foi uma plataforma excelente para o desenvolvimento
do codigo e monitoramento de todas as acoes. Sua velocidade, simplicidade e extensao de
bibliotecas tornaram o trabalho mais simples de ser executado. A utilizacao do MATLAB
para a determinacao dos parametros tambem foi um facilitador na elaboracao do projeto,
pois permitiu que o controlador fosse obtido atraves da observacao da melhor resposta ao
sistema. Como conclusao de todo o trabalho descrito neste relatorio obteve-se um sistema
de robo seguidor de trilha com controlador PID com bons resultados, minimizacao de erros
e desempenho melhor do que com outros controladores testados, como o ON-OFF descrito
anteriormente.
Referencias Bibliograficas
[1] Activity 6 Part (a): Time-Response Analysis of a DC Motor. http://ctms.engin.
umich.edu/CTMS/index.php?aux=AboutTutorials.
[2] S.E.M. Bajestani and A. Vosoughinia. Technical report of building a line follower
robot. In Electronics and Information Engineering (ICEIE), 2010 International Con-
ference On, volume 1, pages V1–1–V1–5, Aug 2010.
[3] K.M. Hasan, A. Al-Nahid, and A. Al Mamun. Implementation of autonomous line
follower robot. In Informatics, Electronics Vision (ICIEV), 2012 International Con-
ference on, pages 865–869, May 2012.
[4] K. Ogata. Engenharia de controle moderno. Pearson Prentice Hall, 1982.
[5] M. Pakdaman and M.M. Sanaatiyan. Design and implementation of line follower ro-
bot. In Computer and Electrical Engineering, 2009. ICCEE ’09. Second International
Conference on, volume 2, pages 585–590, Dec 2009.
[6] M. Pakdaman, M.M. Sanaatiyan, and M.R. Ghahroudi. A line follower robot from de-
sign to implementation: Technical issues and problems. In Computer and Automation
Engineering (ICCAE), 2010 The 2nd International Conference on, volume 1, pages
5–9, Feb 2010.
[7] Wei Wu. Dc motor parameter identification using speed step responses. Model. Simul.
Eng., 2012:30:30–30:30, January 2012.
20
Apendice A
Prototipo Desenvolvido
A Figura A.1 mostra o prototipo desenvolvido.
Figura A.1: Vista superior do prototipo
21
22
Uma placa para os sensores foi montada e pode ser vista na Figura A.2:
Figura A.2: Placa sensorial
Um arranjo experimental foi montado para determinacao da resposta do motor e
pode ser visto na A.3.
Figura A.3: Medicao da velocidade do motor
Apendice B
Codigo PID do Arduino
1 #include <PID_v1.h>
2
3 // Arduino pins for the shift register
4 #define MOTORLATCH 12
5 #define MOTORCLK 4
6 #define MOTORENABLE 7
7 #define MOTORDATA 8
8
9 // 8-bit bus after the 74HC595 shift register
10
11 // These are used to set the direction of the bridge driver.
12 #define MOTOR1_A 2
13 #define MOTOR1_B 3
14 #define MOTOR2_A 1
15 #define MOTOR2_B 4
16
17 // Arduino pins for the PWM signals.
18 #define MOTOR1_PWM 11
19 #define MOTOR2_PWM 3
20
21 // Codes for the motor function.
22 #define FORWARD 1
23 #define BACKWARD 2
24 #define BRAKE 3
25 #define RELEASE 4
26
27 // Tuning parameters
28 float Kp = 1; //Initial Proportional Gain
29 float Ki = 0.22; //Initial Integral Gain
30 float Kd = 0.4; //Initial Differential Gain
31
32
33 void setup()
34
35 //Serial.begin(9600); //Start a serial session
36
37
38
39 int lastError = 0;
23
24
40 int rightBaseSpeed = 45;
41 int leftBaseSpeed = 45;
42 int position = 0;
43
44 void loop()
45
46 int LeftSensorValue = analogRead(A1);
47 int RightSensorValue = analogRead(A0);
48
49 float voltageLeft = LeftSensorValue * (5.0 / 1023.0);
50 float voltageRight = RightSensorValue * (5.0 / 1023.0);
51
52 //Serial.println(voltageLeft);
53 //Serial.println(voltageRight);
54
55 if (voltageRight > 1 && voltageLeft > 2) // nao bateu nenhum
56 position = 2500;
57 else
58 if (voltageLeft > 2) // bateu sensor esquerdo
59 position = 0;
60 else if (voltageRight > 1) // bateu sensor direito
61 position = 5000;
62
63
64
65 Serial.println(position);
66 int error = position - 2500;
67
68 //Serial.println(error);
69 error = map(error, -2500, 2500, -25, 25);
70 int motorSpeed = Kp * error + Kd * (error - lastError);
71 lastError = error;
72
73
74 int rightMotorSpeed = rightBaseSpeed + motorSpeed;
75 int leftMotorSpeed = leftBaseSpeed - motorSpeed;
76
77
78 //Serial.print(" SpeedR = ");
79 //Serial.println(rightMotorSpeed);
80 // Serial.print(" SpeedL = ");
81 //Serial.println(leftMotorSpeed);
82
83 motor(1, FORWARD, rightMotorSpeed);
84 motor(2, FORWARD, leftMotorSpeed);
85
86 //
87 // if(LeftSensorValue > RightSensorValue) //Se bateu do lado esquerdo, o controle e do motor direito
88 //
89 // SetPoint = SetPointR;
90 // Input = map(LeftSensorValue, 0, 1024, 0, 50); //Map it to the right scale
91 //
92 // myPID.Compute(); //Run the PID loop
93 // OutputR = SetPoint + Output;
94 // OutputL = SetPoint - Output;
95 // motor(1, FORWARD, OutputR);
25
96 // motor(2, FORWARD, OutputL);
97 //
98 // else if(LeftSensorValue < RightSensorValue)
99 //
100 // SetPoint = SetPointL;
101 // Input = map(RightSensorValue, 0, 1024, 0, 50);
102 //
103 // myPID.Compute(); //Run the PID loop
104 // OutputR = SetPoint - Output;
105 // OutputL = SetPoint + Output;
106 // motor(1, FORWARD, OutputR);
107 // motor(2, FORWARD, OutputL);
108 //
109
110 //
111 //
112 ////analogWrite(led, OutputR); //Write out the output from the PID loop to our LED pin
113 //now = millis(); //Keep track of time
114 //
115 // if(now - lastMessage > serialPing)
116 // //If it has been long enough give us some info on serial
117 // // this should execute less frequently
118 // // send a message back to the mother ship
119 // Serial.print("SetPoint = ");
120 // Serial.print(SetPoint);
121 // Serial.print(" Input = ");
122 // Serial.print(Input);
123 // Serial.print(" InputR = ");
124 // Serial.print(RightSensorValue);
125 // Serial.print(" InputL = ");
126 // Serial.print(LeftSensorValue);
127 // Serial.print(" OutputR = ");
128 // Serial.print(OutputR);
129 // Serial.print(" OutputL = ");
130 // Serial.print(OutputL);
131 // Serial.print("\n");
132 //
133 // if (Serial.available() > 0)
134 // //If we sent the program a command deal with it
135 // for (int x = 0; x < 4; x++)
136 // switch (x)
137 // case 0: Kp = Serial.parseFloat(); break;
138 // case 1: Ki = Serial.parseFloat(); break;
139 // case 2: Kd = Serial.parseFloat(); break;
140 // case 3:
141 // for (int y = Serial.available(); y == 0; y--)
142 // Serial.read(); //Clear out any residual junk
143 // break;
144 //
145 //
146 // Serial.print(" Kp,Ki,Kd = ");
147 // Serial.print(Kp);
148 // Serial.print(",");
149 // Serial.print(Ki);
150 // Serial.print(",");
151 // Serial.println(Kd); //Let us know what we just received
26
152 // //myPID_R.SetTunings(KpR, KiR, KdR); //Set the PID gain constants and start running
153 //
154 //
155 // lastMessage = now; //update the time stamp.
156 //
157 ////Serial.println(myPID_R.GetKp());
158
159
160
161
162
163
164
165 // motor
166 // Select the motor (1-4), the command,
167 // and the speed (0-255).
168 // The commands are: FORWARD, BACKWARD, BRAKE, RELEASE.
169 void motor(int nMotor, int command, int speed)
170
171 int motorA, motorB;
172
173 if (nMotor >= 1 && nMotor <= 4)
174 switch (nMotor)
175 case 1:
176 motorA = MOTOR1_A;
177 motorB = MOTOR1_B;
178 break;
179 case 2:
180 motorA = MOTOR2_A;
181 motorB = MOTOR2_B;
182 break;
183 default:
184 break;
185
186
187 switch (command)
188 case FORWARD:
189 motor_output (motorA, HIGH, speed);
190 motor_output (motorB, LOW, -1); // -1: no PWM set
191 break;
192 case BACKWARD:
193 motor_output (motorA, LOW, speed);
194 motor_output (motorB, HIGH, -1); // -1: no PWM set
195 break;
196 case BRAKE:
197 motor_output (motorA, LOW, 255); // 255: fully on.
198 motor_output (motorB, LOW, -1); // -1: no PWM set
199 break;
200 case RELEASE:
201 motor_output (motorA, LOW, 0); // 0: output floating.
202 motor_output (motorB, LOW, -1); // -1: no PWM set
203 break;
204 default:
205 break;
206
207
27
208
209
210
211 void motor_output (int output, int high_low, int speed)
212
213 int motorPWM;
214
215 switch (output)
216
217 case MOTOR1_A:
218 case MOTOR1_B:
219 motorPWM = MOTOR1_PWM;
220 break;
221 case MOTOR2_A:
222 case MOTOR2_B:
223 motorPWM = MOTOR2_PWM;
224 break;
225 default:
226 // Use speed as error flag, -3333 = invalid output.
227 speed = -3333;
228 break;
229
230
231 if (speed != -3333)
232
233 // Set the direction with the shift register
234 // on the MotorShield, even if the speed = -1.
235 // In that case the direction will be set, but
236 // not the PWM.
237 shiftWrite(output, high_low);
238
239 // set PWM only if it is valid
240 if (speed >= 0 && speed <= 255)
241
242 analogWrite(motorPWM, speed);
243
244
245
246
247
248 // ---------------------------------
249 // shiftWrite
250 //
251 // The parameters are just like digitalWrite().
252 //
253 // The output is the pin 0...7 (the pin behind
254 // the shift register).
255 // The second parameter is HIGH or LOW.
256 //
257 // There is no initialization function.
258 // Initialization is automatically done at the first
259 // time it is used.
260 //
261 void shiftWrite(int output, int high_low)
262
263 static int latch_copy;
28
264 static int shift_register_initialized = false;
265
266 // Do the initialization on the fly,
267 // at the first time it is used.
268 if (!shift_register_initialized)
269
270 // Set pins for shift register to output
271 pinMode(MOTORLATCH, OUTPUT);
272 pinMode(MOTORENABLE, OUTPUT);
273 pinMode(MOTORDATA, OUTPUT);
274 pinMode(MOTORCLK, OUTPUT);
275
276 // Set pins for shift register to default value (low);
277 digitalWrite(MOTORDATA, LOW);
278 digitalWrite(MOTORLATCH, LOW);
279 digitalWrite(MOTORCLK, LOW);
280 // Enable the shift register, set Enable pin Low.
281 digitalWrite(MOTORENABLE, LOW);
282
283 // start with all outputs (of the shift register) low
284 latch_copy = 0;
285
286 shift_register_initialized = true;
287
288
289 // The defines HIGH and LOW are 1 and 0.
290 // So this is valid.
291 bitWrite(latch_copy, output, high_low);
292
293 // Use the default Arduino ’shiftOut()’ function to
294 // shift the bits with the MOTORCLK as clock pulse.
295 // The 74HC595 shiftregister wants the MSB first.
296 // After that, generate a latch pulse with MOTORLATCH.
297 shiftOut(MOTORDATA, MOTORCLK, MSBFIRST, latch_copy);
298 delayMicroseconds(5); // For safety, not really needed.
299 digitalWrite(MOTORLATCH, HIGH);
300 delayMicroseconds(5); // For safety, not really needed.
301 digitalWrite(MOTORLATCH, LOW);
302