Universidade Federal da Bahia

cnica
Escola Polite
trica
Departamento de Engenharia Ele
trica
Curso de Engenharia Ele
lise de Sistemas Dina
micos
ENGC35 - Modelagem e Ana

Seguidor de Linha
Projeto de um Robo
com Controlador PID

Ana Tereza Borba

Franklin Lima
Lucas Santana

Salvador - Bahia
Novembro de 2015

 Seguidor de Linha
Projeto de um Robo
com 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 Sistemas Dinamicos ministrada pelo professor Humberto Xavier. Inicialmente apresentam-se
as etapas do projeto, descricao dos modelos utilizados e sao apresentadas tambem as descricoes dos projetos de modo conceitual e posteriormente detalhado.

Sum
ario
Introduc
ao

1 Projeto de Engenharia

1.1

Cronograma . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1.2

Orcamento . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1.3

Work Breakdown Structure . . . . . . . . . . . . . . . . . . . . . . . . . .

1.4

RACI Chart . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2 Modelo do Sistema

2.1

Modelo Cinematico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2.2

Modelo do Motor DC . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

2.2.1

2.3

Identificacao dos Parametros . . . . . . . . . . . . . . . . . . . . . .

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 Conclus
oes

18

Refer
encias Bibliogr
aficas

20

APPENDICES

21

A Prot
otipo Desenvolvido

21

B C
odigo PID do Arduino

23

Introdu
c
ao
O controle de sistemas tem sido um objeto de estudos cada vez maior da area de engenharia. O ser humano tem buscado revolucionar os equipamentos e maquinas de modo
nesse
a aperfeicoar o seu funcionamento, tornando-os mais estaveis e suavizando erros. E
contexto que as definicoes de modelagem de sistemas e controle aparecem como protagonistas em diversos cursos: Engenharia Eletrica, Eletronica, Mecanica e, principalmente,
Engenharia de Automacao e Controle.
Estudar controladores significa estudar formas de como realizar um processo, entende-lo, elencar ferramentas e parametros para obter o melhor resultado.

Problema proposto
A Figura 1 mostra um veculo autonomo que deve percorrer uma trajetoria curvilnea. O problema resume-se em manter o carrinho o mais estavel possvel ao realizar
as curvas da trilha atraves de um sistema de controle que leia a entrada da posicao do
veculo e retorne uma resposta de alteracao da velocidade dos motores para que o prototipo
retorne ao set point estabelecido.

Veculo
Sensor de
luminosidade

Figura 1: Vista superior do robo sobre a trajetoria

A organizacao do trabalho e a que segue: no Captulo 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 Captulo 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 Captulo 3 e, por fim, os resultados e conclusoes
sao apesentados no Captulo 4.

Captulo 1
Projeto de Engenharia
Neste captulo 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 captulo 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 incio das atividades foi dado como 12/10/2015.

Tabela 1.1: Cronograma das atividades
Semana
Etapa

Revis
ao bibliogr
afica

Elaborac
ao do projeto conceitual

Elaborac
ao do projeto basico

Busca de facilitadores e recursos

Aquisic
ao de materiais

Identificac
ao dos par
ametros do motor

Identificac
ao do modelo do sensor

Modelagem do sistema

Testes e Correc
oes

Escrita do relat
orio

Apresentac
ao

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

Descric
ao

Qtde.

Preco Unit
ario

Preco Total

Arduino Leonardo

R$ 43,99

R$ 43,99

Shield L293d Driver Ponte H

R$ 22,90

R$ 22,90

Chassi + Motor com reducao

R$ 46,00

R$ 46,00

Sensor CNY70

R$ 4,00

R$ 8,00

Trimpot 10k

R$ 2,50

R$ 5,00

Resistor 10k

R$ 0,20

R$ 0,40

Resistor 100

R$ 0,20

R$ 0,40

Barra de pinos

R$ 2,00

R$ 4,00

10

Jumper

R$ 0,30

R$ 0,90

11

Placa de Fenolite 10x10cm

R$ 7,75

R$ 7,75

12

Bateria 9V

R$ 3,35

R$ 6,70

Total

R$ 146,04

1.3

Work Breakdown Structure

O diagrama WBS do projeto pode ser visto na Figura 1.1
Rob Seguidor de Trilha com Controlador PID

Sensores

Modelagem

Identificao dos
Parmetros

C - Arduino

Trilha

Chassi

Cdigo

Placa de Interface

Figura 1.1: Diagrama WBS para o projeto

Documentao

Motor DC

RACI Chart
A matriz de atribuicao de responsabilidade e atividades e mostrada na Tabela 1.3.

Lucas Santana

Revisao bibliografica

Elaboracao do projeto conceitual e basico

Elaboracao do projeto detalhado

R, A

Busca de facilitadores e recursos

Aquisicao de materiais

Identificacao dos parametros do motor

Identificacao do modelo do sensor

Modelagem do sistema

Tarefas

Humberto

Franklin Lima

Tabela 1.3: RACI Chart para o projeto

Ana Tereza

1.4

R
R, A

R
R

R, A

Testes e Correcoes

10

Escrita da documentacao

R, A

11

Apresentacao

C, I

Legenda
R

Responsavel pela execucao da tarefa. Podem ser uma ou mais pessoas designadas a executar a tarefa.

Prestador de contas. Havera somente uma pessoa designada para

esse papel.

Consultor da tarefa. Sao pessoas com maior know how sobre

determinados assuntos, responsaveis por fornecerem informacoes
u
teis para a conclusao da tarefa. A comunicacao com esse grupo
sera de duas vias.

Pessoas informadas sobre o progresso e status da tarefa. A comunicacao com esse grupo sera de mao u
nica

Captulo 2
Modelo do Sistema
Neste captulo serao descritos os modelos do sistema. Na Secao 2.1 sera descrito o
modelo cinematico, mostrando a relacao entre as dimensoes fsicas da trilha e do prototipo
e o seu comportamento dinamico, enquanto na Secao 2.2 e mostrado o modelo do motor 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 Cinem
atico
Tem-se um veculo dotado de duas rodas com tracao obtida por dois motores de

corrente contnua e uma roda omnidirecional, que com auxlio 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 veculo. Para manter o veculo na curva e preciso
que 0. e, portanto, a variavel controlada.
O objetivo do controlador e, deste modo, e garantir, com bom desempenho, que

Motor Esquerdo

29

Motor Direito

Figura 2.1: Vista superior do robo sobre a trajetoria

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 contnua controlado pela

corrente da armadura e dada pela Eq. 2.1

G(s) =

(s)
Km
Km/(Ra b + Kb Km )
=
=
Va (s)
s[Ra (Js + b) + Kb Km ]
s(1 s + 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.

(2.1)

9
O diagrama em blocos pode ser visto na Figura 2.2.

Figura 2.2: Diagrama em blocos do motor de corrente contnua

Como nao foi possvel 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

Identifica
c
ao dos Par
ametros
De modo a desempenhar a identificacao dos parametros do motor utilizado, foi ne-

cessario realizar experimento com auxlio de um sensor optico, com o objetivo e obter uma
resposta aproximada a` resposta ao degrau do motor. O metodo utilizado esta disponvel
em [1].
O grafico da Figura 2.3 mostra o resultado aproximado da resposta ao degrau do
motor.
Resposta ao Degrau
1

0.8

0.6

0.4

0.2

10

Tempo (s)

Figura 2.3: Resposta ao degrau bastante ruidosa

10
Como e possvel 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 perodo de amostragem nao coincidem precisamente [1].
Com auxlio 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 disponvel no
Apendice A.
Utilizando-se a ferramenta Parameter Estimation do Matlab foi possvel a estimacao 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 disponvel 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 superfcie.
I Crel - Relative Collector Current

1.0
0.9

1.5

0.8

d = 5 mm
4 mm
3 mm
2 mm
1 mm
0

0.7
0.6
0.5
0.4

10 mm

10 mm

VCE = 5 V
I F = 20 mA

0.2
0.1
1

0
5 mm

0
5 mm

0.3

0.0

10

11

s - Displacement (mm)

96 11915

Figura 2.7: Corrente eletrica relativa versus distancia

Embora o sensor responda continuamente `a distancia, podemos definir limites de
modo que:
(
VOU T =

0, VOU T < 2V
1, cc.

Onde 1 e 0 representam nveis logicos para famlia TTL.

Captulo 3
Projeto do Controlador
O controlador escolhido para ser utilizado no projeto e o PID (ProporcionalIntegral-Derivativo), que e o algoritmo de controle mais usado na ind
ustria e tem sido
utilizado em todo o mundo para sistemas de controle industrial. A popularidade de controladores PID pode ser atribuda 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:
Z

u(t) = Kp e(t) + Ki

e( )d + Kd
0

d
e(t)
dt

(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
auxlio do MATLAB. A Figura 3.1 mostra parte do processo. Basicamente, o MATLAB,
utilizando-se do modelo linearizado da planta, gera a sada 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 B
asico
O projeto de um robo seguidor de linha com controlador PID consiste na construcao

de um prototipo de veculo autonomo cujo objetivo e percorrer uma trajetoria curvilnea

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 veculo e a sada do sensor,
funcao do controlador alterar a
que determina a atual posicao do veculo na trilha. E
velocidade dos motores de modo que o veculo 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

Referncia

PID

Interface

Ponte H

Sensores

Motor
Direito

Motor
Esquerdo

Interface
Sensor

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 Mec
anica
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 El
etrica

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 interfaceamento 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 veculo 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

Sensor CNY70

R2

100

4.7k

5V

5V

VOUT
10k

R3

Figura 3.6: Circuito de interface para o sensor

Implementac
ao 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 disponvel 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
Simulac
ao
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

Captulo 4
Resultados e Conclus
oes
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 fsicos, 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 superfcie 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 possvel suavizar a trajetoria do robo
consideravelmente para que ele seguisse a trilha de forma aperfeicoada, diminuindo o erro
do sistema na trajetoria. O Arduno 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.

Refer
encias Bibliogr
aficas
[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 Conference On, volume 1, pages V11V15, 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 Conference on, pages 865869, 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 robot. In Computer and Electrical Engineering, 2009. ICCEE 09. Second International
Conference on, volume 2, pages 585590, Dec 2009.
[6] M. Pakdaman, M.M. Sanaatiyan, and M.R. Ghahroudi. A line follower robot from design to implementation: Technical issues and problems. In Computer and Automation
Engineering (ICCAE), 2010 The 2nd International Conference on, volume 1, pages
59, Feb 2010.
[7] Wei Wu. Dc motor parameter identification using speed step responses. Model. Simul.
Eng., 2012:30:3030:30, January 2012.

20

Ap
endice A
Prot
otipo 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

Ap
endice B
C
odigo 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
57

position = 2500;
} else {

58

if (voltageLeft > 2) // bateu sensor esquerdo

59

position = 0;

60

else if (voltageRight > 1) // bateu sensor direito

61
62

position = 5000;
}

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

//

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

// }

motor(2, FORWARD, OutputL);

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

//

134

//

//If we sent the program a command deal with it

135

//

for (int x = 0; x < 4; x++) {

136

//

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

//

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

if (Serial.available() > 0) {

switch (x) {

} break;
}

26
152

//

153

//

154

//

155

//

156

//

157

////Serial.println(myPID_R.GetKp());

//myPID_R.SetTunings(KpR, KiR, KdR); //Set the PID gain constants and start running

} lastMessage = now; //update the time stamp. }

}

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
229

break;
}

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
287

shift_register_initialized = true;
}

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 }