-
Notifications
You must be signed in to change notification settings - Fork 0
/
robos_joystick.cpp
197 lines (166 loc) · 10.9 KB
/
robos_joystick.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
#include <unistd.h>
#include <stdlib.h>
#include "joystick.hh"
#include "radio.hpp"
enum MODO_OPERACAO {
CONTROLE_ANALOGICO, /* Usaremos o analogico para controlar o sentido do robo (esquerda direita) e o L2 e R2 para controlar a direção (Ré e Frente, respectivamente). */
CONTROLE_LR, /* Usaremos apenas os botões L e R para o controle dos robos. Os botões L (L1 para trás L2 para frente) controla a roda esquerda e os botões R (R1 para trás R2 para frente) controlam a roda direita. */
/* ADICIONAR NOVOS MODOS DE OPERAÇÃO ANTES DE TOTAL_MODO_OPERACAO. Ao adicionar novos elementos acima, atualizar o README.md na seção 'Rodando o programa', Colocando o limite de MODO_OPERACAO igual TOTAL_MODO_OPERACAO - 1. */
TOTAL_MODO_OPERACAO /* 2 */
};
static const int NUM_ROBOS = 3; /**< LEGACY: MANTER EM 3 ENQUANTO O CODIGO DO ARDUINO NAO FOR CORRIGIDO - Deve ser igual ao esperado no código dos arduinos (pois o tamanho do buffer não variável). Ao alterar esta variável, atualizar também o README.md na seção 'Rodando o programa', corrigindo o numero máximo de NUM_JOYSTICKS para ao igual a NUM_ROBOS. */
static const unsigned char MAX_VELOCIDADE_FRENTE = 7; /**< 0111(7). Vai para frente (bit mais significativo indica sentido da rotacao) com velocidade maxima */
static const unsigned char MAX_VELOCIDADE_TRAZ = 15; /**< 0111(7) or 1000(8) = 1111(15). Vai para tras (bit mais significativo indica sentido da rotacao) com velocidade maxima */
struct botoesControle {
/* Nomeando os botões realcionados à struct JoystickEvent::number */
/* Elementos relacionados à JoystickEvent::type = 1 == JS_EVENT_BUTTON */
static const unsigned char BOTAO_L1 = 6;
static const unsigned char BOTAO_R1 = 7;
static const unsigned char BOTAO_L2 = 4;
static const unsigned char BOTAO_R2 = 5;
/* Elementos relacionados à JoystickEvent::type = 2 == JS_EVENT_AXIS */
static const unsigned char AXIS_ESQ_H = 2; /* analogico esquerdo movintos horizontais (eixo X) */
bool b_L1; /**< se o botao L1 estiver precionado b_L1 = 1, senao b_L1 = 0 */
bool b_R1; /**< se o botao R1 estiver precionado b_R1 = 1, senao b_R1 = 0 */
bool b_L2; /**< se o botao L2 estiver precionado b_L2 = 1, senao b_L2 = 0 */
bool b_R2; /**< se o botao R2 estiver precionado b_R2 = 1, senao b_R2 = 0 */
int a_esq_h; /* valores do analogico esquerdo movimento horizontal */
};
struct Controle{
botoesControle botoes_pressionados; /**< indica quais botoes do controle estao pressionados no momento */
Joystick *joystick; /**< Ponteiro para instancia de joystick */
};
int main(int argc, char const *argv[]) {
if (argc != 3) {
std::cerr << "ERROR: Numero errado de argumentos. " << "Numero de argumentos: " << argc << ". Uso: './Robos_Joystick NUM_JOYSTICKS MODO_OPERACAO' onde NUM_JOYSTICKS eh o numero de controles a serem usados. Sao aceitos valores numericos de 1 a " << NUM_ROBOS << " (inclusivo para ambas extremidades [1 a " << NUM_ROBOS << "]) e MODO_OPERACAO valores numericos de 0 a " << TOTAL_MODO_OPERACAO - 1 << std::endl << std::endl << "TOTAL_MODO_OPERACAO = 0: Usaremos o analogico para controlar o sentido do robo (esquerda direita) e o L2 e R2 para controlar a direção (Ré e Frente, respectivamente). "<< std::endl << "TOTAL_MODO_OPERACAO = 1: Usaremos apenas os botões L e R para o controle dos robos. Os botões L (L1 para trás L2 para frente) controla a roda esquerda e os botões R (R1 para trás R2 para frente) controlam a roda direita." << std::endl << "Saindo..." << std::endl;
exit(1);
}
/* convertendo string do argumento (argv[1]) para um inteiro base 10 e ignorando o restante dos argumentos (NULL). */
/**< LEGACY: ATE ARRUMARMOS O CODIDO DO ARDUINO (TX E RX) PRECISAMOS DEFINIR O NUMERO DE JOYSTICKS CONECTADOS AO COMPUTADOR.*/
const int NUM_JOYSTICKS = strtol(argv[1], NULL, 10);
if(NUM_JOYSTICKS <= 0 || NUM_JOYSTICKS > 3) {
std::cerr << "ERROR: Numero invalido de joysticks." << " Uso: './Robos_Joystick NUM_JOYSTICKS' onde NUM_JOYSTICKS eh o numero de controles a serem usados. Deve sao aceitos valores de 1 a 3 (inclusivo para ambas extremidades [1 a 3])." << std::endl << "Saindo..." << std::endl;
exit(1);
}
const int modoOP = strtol(argv[2], NULL, 10);
if(modoOP < 0 || modoOP >= TOTAL_MODO_OPERACAO) {
std::cerr << "ERROR: Modo de operacao invalido." << "MODO_OPERACAO valores numericos de 0 a " << TOTAL_MODO_OPERACAO - 1 << std::endl << std::endl << "TOTAL_MODO_OPERACAO = 0: Usaremos o analogico para controlar o sentido do robo (esquerda direita) e o L2 e R2 para controlar a direção (Ré e Frente, respectivamente). "<< std::endl << "TOTAL_MODO_OPERACAO = 1: Usaremos apenas os botões L e R para o controle dos robos. Os botões L (L1 para trás L2 para frente) controla a roda esquerda e os botões R (R1 para trás R2 para frente) controlam a roda direita." << std::endl << "Saindo..." << std::endl;
exit(1);
}
std::vector<Controle> controle(NUM_JOYSTICKS); /**< aloca o vetor de structs de joysticks e os botoes que estao precionados no joystick. */
for(int i = 0 ; i < NUM_JOYSTICKS; i++)
controle[i].joystick = new Joystick(i); /**< instancia do joystic i controlara o robo i. */
std::vector<Robo> robos(NUM_ROBOS); /**< vetor com os NUM_ROBOS robos. */
Radio radio(robos); /**< instancia de radio para que possamos enviar os comandos para os robos. */
// deteccao dos joysticks
for(int i = 0; i < NUM_JOYSTICKS; i++) {
// Determinando se eh possivel detectar e utilizar o joystick i
if (!controle[i].joystick->isFound()) {
printf("Joystick %d nao encontrado. Saindo...\n", i + 1);
exit(1);
}
}
/* LOOP Principal */
radio.comecaComunicacao(); /* inicializa a thread temporizada que fará o envio dos dados aos robos. */
while (true) {
// restricao de tempo para fazermos a leitura e envio das velocidades aos robos
usleep(Radio::VELOCIDADE_SERIAL);
// para cada robo e controle determinar quais os botoes estao precionados e quais nao estao (ou foram liberados agora)
for(int i = 0; i < NUM_JOYSTICKS; i++) {
JoystickEvent evento_joystick_i;
/* BEGIN DEBUG */
// fazendo a leitura dos dados dos controles
// std::cout << evento_joystick_i << '\n';
/* END DEBUG */
/* DETECÇÃO DE EVENTOS DISPARADOS PELO JOYSTICK i */
if (controle[i].joystick->sample(&evento_joystick_i)) {
// Detectando se eh um botao que esta sendo pressionado ou liberado (pode ser um dos analogicos disparando o evento)
if (evento_joystick_i.isButton()) {
// detectando qual botao esta sendo pressionado ou liberado
switch (evento_joystick_i.number) {
// L1 esta sendo pressionado
case botoesControle::BOTAO_L1:
// se for true, significa que o botao esta sendo precionado agora, logo controle[i].botoes_pressionados.b_L1 eh true, do contrario esta sendo liberado.
evento_joystick_i.value == true ? controle[i].botoes_pressionados.b_L1 = true : controle[i].botoes_pressionados.b_L1 = false;
break;
// R1 esta sendo pressionado
case botoesControle::BOTAO_R1:
// se for true, significa que o botao esta sendo precionado agora, logo controle[i].botoes_pressionados.b_R1 eh true, do contrario esta sendo liberado.
evento_joystick_i.value == true ? controle[i].botoes_pressionados.b_R1 = true : controle[i].botoes_pressionados.b_R1 = false;
break;
// L2 esta sendo pressionado
case botoesControle::BOTAO_L2:
// se for true, significa que o botao esta sendo precionado agora, logo controle[i].botoes_pressionados.b_L2 eh true, do contrario esta sendo liberado.
evento_joystick_i.value == true ? controle[i].botoes_pressionados.b_L2 = true : controle[i].botoes_pressionados.b_L2 = false;
break;
// R2 esta sendo pressionado
case botoesControle::BOTAO_R2:
// se for true, significa que o botao esta sendo precionado agora, logo controle[i].botoes_pressionados.b_R2 eh true, do contrario esta sendo liberado.
evento_joystick_i.value == true ? controle[i].botoes_pressionados.b_R2 = true : controle[i].botoes_pressionados.b_R2 = false;
break;
}
}
else if(evento_joystick_i.isAxis()) {
switch (evento_joystick_i.number) {
case botoesControle::AXIS_ESQ_H:
controle[i].botoes_pressionados.a_esq_h = evento_joystick_i.value;
break;
}
}
}
}
/* SETANDO OS VALORES DE VELOCIDADE DOS ROBOS */
for(int i = 0; i < NUM_JOYSTICKS; i++) {
unsigned char rodaEsquerda = 0; /**< indica a velocidade atual da roda esquerda. */
unsigned char rodaDireita = 0; /**< indica a velocidade atual da roda direita. */
double modificador;
switch (modoOP) {
case CONTROLE_LR:
// se L1 esta precionado, a roda esquerda do robo gira para tras
if(controle[i].botoes_pressionados.b_L1)
rodaEsquerda = MAX_VELOCIDADE_TRAZ;
// se L2 esta precionado, a roda esquerda do robo gira para frente
if(controle[i].botoes_pressionados.b_L2)
rodaEsquerda = MAX_VELOCIDADE_FRENTE;
// se R1 esta precionado, a roda direita do robo gira para tras
if(controle[i].botoes_pressionados.b_R1)
rodaDireita = MAX_VELOCIDADE_TRAZ;
// se R2 esta precionado, a roda direita do robo gira para frente
if(controle[i].botoes_pressionados.b_R2)
rodaDireita = MAX_VELOCIDADE_FRENTE;
break;
case CONTROLE_ANALOGICO:
// se R2 esta precionado, o robo vai para frente
if(controle[i].botoes_pressionados.b_R2){
rodaEsquerda = MAX_VELOCIDADE_FRENTE;
rodaDireita = MAX_VELOCIDADE_FRENTE;
}
// se L2 esta precionado, o robo para tras
if(controle[i].botoes_pressionados.b_L2) {
rodaEsquerda= MAX_VELOCIDADE_TRAZ;
rodaDireita = MAX_VELOCIDADE_TRAZ;
}
modificador = controle[i].botoes_pressionados.a_esq_h / (double)JoystickEvent::MAX_AXES_VALUE;
/* BEGIN DEBUG */
// std::cout << modificador << '\n';
/* END DEBUG */
/* caso modificador seja zero, nao fazemos alteracao nenhuma nas velocidades (reto) */
/* inclinando o analogico para a direita */
if(modificador > 0) {
rodaDireita = rodaDireita * (1 - modificador);
}
/* inclinando o analogico para a esquerda */
else if (modificador < 0) {
modificador = (modificador * -1); /* deixando o modificador positivo */
rodaEsquerda = rodaEsquerda * (1 - modificador);
}
break;
}
/* BEGIN DEBUG */
// std::cout << (int)rodaEsquerda<< " " << (int)rodaDireita<< '\n';
/* END DEBUG */
// gravando valores de velocidade nos robos
robos[i].setVelocidadeAtualRobo(rodaEsquerda, rodaDireita);
}
}
}