Victor Mirkhan / Mbed 2 deprecated PROJETO

Como o projeto está até agora....

Dependencies:   Servo TCS3200 X_NUCLEO_IHM01A1 mbed TextLCD2

Fork of QEI_HelloWorld by Aaron Berk

main.cpp@2:666ad168bacb, 2017-05-12 (annotated)

Committer:
Victor_Mirkhan
Date:
Fri May 12 18:46:45 2017 +0000
Revision:
2:666ad168bacb
Parent:
1:30696e4d196b
Child:
3:77d4b108568c
Projeto at? agora...;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Victor_Mirkhan 2:666ad168bacb 1 #include "mbed.h"
Victor_Mirkhan 2:666ad168bacb 2 #include "RPG.h"
Victor_Mirkhan 2:666ad168bacb 3 #include "L6474.h"
Victor_Mirkhan 2:666ad168bacb 4
Victor_Mirkhan 2:666ad168bacb 5 // Seta o encoder utilizado. Precisa de duas entradas (D14 e D15) e uma terceira
Victor_Mirkhan 2:666ad168bacb 6 // que seria o seu botão. Como nosso encoder não tem um botão, declara-se qualquer
Victor_Mirkhan 2:666ad168bacb 7 // pino, já que nem será utilizado.
Victor_Mirkhan 2:666ad168bacb 8
Victor_Mirkhan 2:666ad168bacb 9 RPG rpg1(D14,D15,PC_13);
Victor_Mirkhan 2:666ad168bacb 10
Victor_Mirkhan 2:666ad168bacb 11 /*-----Declaração dos pinos e suas funções-----*/
Victor_Mirkhan 2:666ad168bacb 12
Victor_Mirkhan 2:666ad168bacb 13 //PINOS DE INTERRUPÇÃO.
Victor_Mirkhan 2:666ad168bacb 14
Victor_Mirkhan 2:666ad168bacb 15 // Pinos D15 e D14 são ligados ao encoder e setados como pinos de interrupção.
Victor_Mirkhan 2:666ad168bacb 16 InterruptIn encod1(D15);
Victor_Mirkhan 2:666ad168bacb 17 InterruptIn encod2(D14);
Victor_Mirkhan 2:666ad168bacb 18
Victor_Mirkhan 2:666ad168bacb 19 // Botão do usuario é setado para interrupção, especialmente para a fase de
Victor_Mirkhan 2:666ad168bacb 20 // prototipação.
Victor_Mirkhan 2:666ad168bacb 21 InterruptIn usuario(PC_13);
Victor_Mirkhan 2:666ad168bacb 22
Victor_Mirkhan 2:666ad168bacb 23 //PINOS DE SAIDA DIGITAL.
Victor_Mirkhan 2:666ad168bacb 24
Victor_Mirkhan 2:666ad168bacb 25 // Declara o Led da placa como saída digital.
Victor_Mirkhan 2:666ad168bacb 26 DigitalOut Led(D13);
Victor_Mirkhan 2:666ad168bacb 27
Victor_Mirkhan 2:666ad168bacb 28 /*-----Declaração de variaveis globais do código-----*/
Victor_Mirkhan 2:666ad168bacb 29
Victor_Mirkhan 2:666ad168bacb 30 bool refer1 = false; // Parametro para o referenciamento do motor 1
Victor_Mirkhan 2:666ad168bacb 31 bool refer2 = false; // Parametro para o referenciamento do motor 2
Victor_Mirkhan 2:666ad168bacb 32 bool refer3 = false; // Parametro para o referenciamento do motor 3
Victor_Mirkhan 2:666ad168bacb 33
Victor_Mirkhan 2:666ad168bacb 34 int count; // Contagem de pulsos do encoder
Victor_Mirkhan 2:666ad168bacb 35 int change = 0; // Numero que varia de 0 a 2, indicando qual set de velocidade será usado
Victor_Mirkhan 2:666ad168bacb 36 int dirt = 0; // Variavel que pega o estado presente do encoder
Victor_Mirkhan 2:666ad168bacb 37
Victor_Mirkhan 2:666ad168bacb 38 // Criação de uma struct (basicamente, uma classe) para sets de velocidades e acelerações
Victor_Mirkhan 2:666ad168bacb 39 // diferentes. Cada objeto dessa struct possui os argumentos de velocidades max e min e
Victor_Mirkhan 2:666ad168bacb 40 // aceleração e desaceleração.
Victor_Mirkhan 2:666ad168bacb 41
Victor_Mirkhan 2:666ad168bacb 42 struct set_velocidades {
Victor_Mirkhan 2:666ad168bacb 43 int maxspeed;
Victor_Mirkhan 2:666ad168bacb 44 int minspeed;
Victor_Mirkhan 2:666ad168bacb 45 int ac;
Victor_Mirkhan 2:666ad168bacb 46 int dc;
Victor_Mirkhan 2:666ad168bacb 47 };
Victor_Mirkhan 2:666ad168bacb 48
Victor_Mirkhan 2:666ad168bacb 49 struct set_velocidades set[3]; //Cria um objeto da struct set_velocidades com três
Victor_Mirkhan 2:666ad168bacb 50 //objetos dentro dele (basicamente, um objeto triplo).
Victor_Mirkhan 2:666ad168bacb 51
Victor_Mirkhan 2:666ad168bacb 52 //Struct para sets de coordenadas, com argumentos de posição em x,y e z.
Victor_Mirkhan 2:666ad168bacb 53 struct Coordenadas {
Victor_Mirkhan 2:666ad168bacb 54 int posx;
Victor_Mirkhan 2:666ad168bacb 55 int posy;
Victor_Mirkhan 2:666ad168bacb 56 int posz;
Victor_Mirkhan 2:666ad168bacb 57 };
Victor_Mirkhan 2:666ad168bacb 58
Victor_Mirkhan 2:666ad168bacb 59 struct Coordenadas PickPos,DropPos[3]; //Cria objeto unico para posição de pick, e
Victor_Mirkhan 2:666ad168bacb 60 //objeto triplo para posição de place.
Victor_Mirkhan 2:666ad168bacb 61
Victor_Mirkhan 2:666ad168bacb 62 // Declaração dos motores utilizados.
Victor_Mirkhan 2:666ad168bacb 63
Victor_Mirkhan 2:666ad168bacb 64 L6474 *motor1;
Victor_Mirkhan 2:666ad168bacb 65 L6474 *motor2;
Victor_Mirkhan 2:666ad168bacb 66 L6474 *motor3;
Victor_Mirkhan 2:666ad168bacb 67
Victor_Mirkhan 2:666ad168bacb 68 /*-----Declaração de funções auxiliares----*/
Victor_Mirkhan 2:666ad168bacb 69
Victor_Mirkhan 2:666ad168bacb 70 // Função chamada ao atingir o sensor fim de curso. Para o motor e aciona o zera-
Victor_Mirkhan 2:666ad168bacb 71 // mento ao alterar o valor da variavel refer.
Victor_Mirkhan 2:666ad168bacb 72
Victor_Mirkhan 2:666ad168bacb 73 void motor1_off(void) {
Victor_Mirkhan 2:666ad168bacb 74 motor1->hard_stop();
Victor_Mirkhan 2:666ad168bacb 75 printf("Interrupcao Feita!\r\n");
Victor_Mirkhan 2:666ad168bacb 76 refer1 = true;
Victor_Mirkhan 2:666ad168bacb 77 }
Victor_Mirkhan 2:666ad168bacb 78
Victor_Mirkhan 2:666ad168bacb 79 // Função chamada na interrupção de um dos sentidos de rotação do encoder. Basicamente,
Victor_Mirkhan 2:666ad168bacb 80 // desabilita as interrupções enquanto opera ("disable_irq()"), aumenta o contador e
Victor_Mirkhan 2:666ad168bacb 81 // depois ativa as interrupções novamente ("enable_irq()").
aberk 0:bcff39fac858 82
Victor_Mirkhan 2:666ad168bacb 83 void mais(void){
Victor_Mirkhan 2:666ad168bacb 84 encod2.disable_irq();
Victor_Mirkhan 2:666ad168bacb 85 encod1.disable_irq();
Victor_Mirkhan 2:666ad168bacb 86 count = count + 1 ;
Victor_Mirkhan 2:666ad168bacb 87 wait(0.002);
Victor_Mirkhan 2:666ad168bacb 88 encod1.enable_irq();
Victor_Mirkhan 2:666ad168bacb 89 encod2.enable_irq();
Victor_Mirkhan 2:666ad168bacb 90 }
Victor_Mirkhan 2:666ad168bacb 91
Victor_Mirkhan 2:666ad168bacb 92 // Função chamada na interrupção de um dos sentidos de rotação do encoder. Basicamente,
Victor_Mirkhan 2:666ad168bacb 93 // desabilita as interrupções enquanto opera ("disable_irq()"), diminui o contador e
Victor_Mirkhan 2:666ad168bacb 94 // depois ativa as interrupções novamente ("enable_irq()").
Victor_Mirkhan 2:666ad168bacb 95
Victor_Mirkhan 2:666ad168bacb 96 void menos(void){
Victor_Mirkhan 2:666ad168bacb 97 encod2.disable_irq();
Victor_Mirkhan 2:666ad168bacb 98 encod1.disable_irq();
Victor_Mirkhan 2:666ad168bacb 99 count = count - 1;
Victor_Mirkhan 2:666ad168bacb 100 wait(0.002);
Victor_Mirkhan 2:666ad168bacb 101 encod1.enable_irq();
Victor_Mirkhan 2:666ad168bacb 102 encod2.enable_irq();
Victor_Mirkhan 2:666ad168bacb 103 }
Victor_Mirkhan 2:666ad168bacb 104
Victor_Mirkhan 2:666ad168bacb 105 // Função que seta para o motor 1 suas velocidades máxima e minima, aceleração
Victor_Mirkhan 2:666ad168bacb 106 // e desaceleração.
Victor_Mirkhan 2:666ad168bacb 107
Victor_Mirkhan 2:666ad168bacb 108 void set_aceleracoes(int maxspeed,int minspeed,int ac,int dc) {
Victor_Mirkhan 2:666ad168bacb 109 motor1->set_max_speed(maxspeed);
Victor_Mirkhan 2:666ad168bacb 110 motor1->set_min_speed(minspeed);
Victor_Mirkhan 2:666ad168bacb 111 motor1->set_acceleration(ac);
Victor_Mirkhan 2:666ad168bacb 112 motor1->set_deceleration(dc);
Victor_Mirkhan 2:666ad168bacb 113 }
Victor_Mirkhan 2:666ad168bacb 114
Victor_Mirkhan 2:666ad168bacb 115 void save_pick(void) {
Victor_Mirkhan 2:666ad168bacb 116 PickPos.posx = motor1->get_position();
Victor_Mirkhan 2:666ad168bacb 117 PickPos.posy = motor2->get_position();
Victor_Mirkhan 2:666ad168bacb 118 PickPos.posz = motor3->get_position();
Victor_Mirkhan 2:666ad168bacb 119 printf("Posicao de Pick:\r\n");
Victor_Mirkhan 2:666ad168bacb 120 printf("X = %d, Y = %d, Z = %d\r\n",PickPos.posx,PickPos.posy,PickPos.posz);
Victor_Mirkhan 2:666ad168bacb 121 }
Victor_Mirkhan 2:666ad168bacb 122 //Função que é chamada logo na inicialização do programa para realizar o zera-
Victor_Mirkhan 2:666ad168bacb 123 // mento das posições dos motores.
Victor_Mirkhan 2:666ad168bacb 124
Victor_Mirkhan 2:666ad168bacb 125 void seta_origem() {
Victor_Mirkhan 2:666ad168bacb 126
Victor_Mirkhan 2:666ad168bacb 127 set_aceleracoes(7000,2000,1000,500); //Seta uma alta aceleração para o referenciamento
Victor_Mirkhan 2:666ad168bacb 128 usuario.rise(&motor1_off); //Chamada do fim de curso para a função de interrupção
Victor_Mirkhan 2:666ad168bacb 129
Victor_Mirkhan 2:666ad168bacb 130 while(1) {
Victor_Mirkhan 2:666ad168bacb 131
Victor_Mirkhan 2:666ad168bacb 132 //Motor continua andando em uma só direção enquanto variavel refer1 estiver
Victor_Mirkhan 2:666ad168bacb 133 // em FALSE
Victor_Mirkhan 2:666ad168bacb 134
Victor_Mirkhan 2:666ad168bacb 135 if (refer1 == false){
Victor_Mirkhan 2:666ad168bacb 136 motor1->run(StepperMotor::BWD);
Victor_Mirkhan 2:666ad168bacb 137 motor1->wait_while_active();
Victor_Mirkhan 2:666ad168bacb 138 }
Victor_Mirkhan 2:666ad168bacb 139
Victor_Mirkhan 2:666ad168bacb 140 // Se a interrupção for chamada, a variavel refer1 se torna TRUE, acionando
Victor_Mirkhan 2:666ad168bacb 141 // os comandos a seguir.
Victor_Mirkhan 2:666ad168bacb 142
Victor_Mirkhan 2:666ad168bacb 143 else {
Victor_Mirkhan 2:666ad168bacb 144 motor1->move(StepperMotor::FWD,1000); //Leve recuo
Victor_Mirkhan 2:666ad168bacb 145 motor1->wait_while_active();
Victor_Mirkhan 2:666ad168bacb 146 motor1->set_home(); // Seta posição de Home
Victor_Mirkhan 2:666ad168bacb 147 int HomePosition = motor1->get_position();
Victor_Mirkhan 2:666ad168bacb 148 printf("Posicao Home = %d\r\n",HomePosition); //Verificar que HomePosition = 0
Victor_Mirkhan 2:666ad168bacb 149 refer1 = false;
Victor_Mirkhan 2:666ad168bacb 150 break; //Quebra do loop while, pois referenciamento do motor foi feito
Victor_Mirkhan 2:666ad168bacb 151 }
Victor_Mirkhan 2:666ad168bacb 152
Victor_Mirkhan 2:666ad168bacb 153 }
Victor_Mirkhan 2:666ad168bacb 154 }
Victor_Mirkhan 2:666ad168bacb 155
Victor_Mirkhan 2:666ad168bacb 156 // Função chamada para alternar entre os sets de velocidade.
Victor_Mirkhan 2:666ad168bacb 157
Victor_Mirkhan 2:666ad168bacb 158 void muda_velocidade() {
Victor_Mirkhan 2:666ad168bacb 159 change = change + 1;
Victor_Mirkhan 2:666ad168bacb 160 if (change == 3) {
Victor_Mirkhan 2:666ad168bacb 161 change = 0;
Victor_Mirkhan 2:666ad168bacb 162 }
Victor_Mirkhan 2:666ad168bacb 163 printf("Velocidade %d\r\n",change + 1);
Victor_Mirkhan 2:666ad168bacb 164 set_aceleracoes(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
Victor_Mirkhan 2:666ad168bacb 165 }
aberk 0:bcff39fac858 166
aberk 0:bcff39fac858 167 int main() {
Victor_Mirkhan 2:666ad168bacb 168
Victor_Mirkhan 2:666ad168bacb 169 //Prepara os 3 sets de velocidade, com velocidaes e acelerações variadas.
Victor_Mirkhan 2:666ad168bacb 170
Victor_Mirkhan 2:666ad168bacb 171 set[0].maxspeed = 2000;
Victor_Mirkhan 2:666ad168bacb 172 set[0].minspeed = 1000;
Victor_Mirkhan 2:666ad168bacb 173 set[0].ac = 500;
Victor_Mirkhan 2:666ad168bacb 174 set[0].dc = 500;
Victor_Mirkhan 2:666ad168bacb 175
Victor_Mirkhan 2:666ad168bacb 176 set[1].maxspeed = 4000;
Victor_Mirkhan 2:666ad168bacb 177 set[1].minspeed = 2000;
Victor_Mirkhan 2:666ad168bacb 178 set[1].ac = 750;
Victor_Mirkhan 2:666ad168bacb 179 set[1].dc = 750;
Victor_Mirkhan 2:666ad168bacb 180
Victor_Mirkhan 2:666ad168bacb 181 set[2].maxspeed = 7000;
Victor_Mirkhan 2:666ad168bacb 182 set[2].minspeed = 5000;
Victor_Mirkhan 2:666ad168bacb 183 set[2].ac = 1000;
Victor_Mirkhan 2:666ad168bacb 184 set[2].dc = 1000;
Victor_Mirkhan 2:666ad168bacb 185
Victor_Mirkhan 2:666ad168bacb 186
Victor_Mirkhan 2:666ad168bacb 187 DevSPI dev_spi(D11, D12, D13);
aberk 0:bcff39fac858 188
Victor_Mirkhan 2:666ad168bacb 189 //Inicialização dos componentes dos motores
Victor_Mirkhan 2:666ad168bacb 190 motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi);
Victor_Mirkhan 2:666ad168bacb 191 motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi);
Victor_Mirkhan 2:666ad168bacb 192 motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi);
Victor_Mirkhan 2:666ad168bacb 193
Victor_Mirkhan 2:666ad168bacb 194 if (motor1->init() != COMPONENT_OK) {
Victor_Mirkhan 2:666ad168bacb 195 exit(EXIT_FAILURE);
Victor_Mirkhan 2:666ad168bacb 196 }
Victor_Mirkhan 2:666ad168bacb 197 if (motor2->init() != COMPONENT_OK) {
Victor_Mirkhan 2:666ad168bacb 198 exit(EXIT_FAILURE);
Victor_Mirkhan 2:666ad168bacb 199 }
Victor_Mirkhan 2:666ad168bacb 200 if (motor3->init() != COMPONENT_OK) {
Victor_Mirkhan 2:666ad168bacb 201 exit(EXIT_FAILURE);
aberk 0:bcff39fac858 202 }
Victor_Mirkhan 2:666ad168bacb 203
Victor_Mirkhan 2:666ad168bacb 204 // Após inicializar tudo, inicia o zeramento.
Victor_Mirkhan 2:666ad168bacb 205 seta_origem();
Victor_Mirkhan 2:666ad168bacb 206
Victor_Mirkhan 2:666ad168bacb 207 //Seta a velocidade inicial como a primeira combinação do set
Victor_Mirkhan 2:666ad168bacb 208 set_aceleracoes(set[0].maxspeed,set[0].minspeed,set[0].ac,set[1].dc);
Victor_Mirkhan 2:666ad168bacb 209
Victor_Mirkhan 2:666ad168bacb 210 while(1) {
Victor_Mirkhan 2:666ad168bacb 211
Victor_Mirkhan 2:666ad168bacb 212 //Declara as funções a serem chamadas nas bordas se subida de cada entrada.
Victor_Mirkhan 2:666ad168bacb 213
Victor_Mirkhan 2:666ad168bacb 214 encod1.rise(&mais);
Victor_Mirkhan 2:666ad168bacb 215 encod2.rise(&menos);
Victor_Mirkhan 2:666ad168bacb 216 usuario.rise(&muda_velocidade);
Victor_Mirkhan 2:666ad168bacb 217 //usuario.rise(&save_pick);
Victor_Mirkhan 2:666ad168bacb 218
Victor_Mirkhan 2:666ad168bacb 219 /*---PAR DE LOOPS PARA O JOG---*/
Victor_Mirkhan 2:666ad168bacb 220
Victor_Mirkhan 2:666ad168bacb 221 // Esse loop faz com que, caso e enquanto o contador for menor que zero,
Victor_Mirkhan 2:666ad168bacb 222 // o motor proporcione o movimento BACKWARD.
Victor_Mirkhan 2:666ad168bacb 223
Victor_Mirkhan 2:666ad168bacb 224 if (count < 0) {
Victor_Mirkhan 2:666ad168bacb 225 while(count < 0) {
Victor_Mirkhan 2:666ad168bacb 226 motor1->run(StepperMotor::BWD);
Victor_Mirkhan 2:666ad168bacb 227 wait(0.1);
Victor_Mirkhan 2:666ad168bacb 228 count = count + 1; //Acrescenta 1 ao contador a cada 0.1seg, aproximando-o de zero.
Victor_Mirkhan 2:666ad168bacb 229 if (count >= 0) { //Se o contador mudar para 0 ou positivo, motor para e quebra o while.
Victor_Mirkhan 2:666ad168bacb 230 motor1->hard_stop();
Victor_Mirkhan 2:666ad168bacb 231 count = 0;
Victor_Mirkhan 2:666ad168bacb 232 break;
Victor_Mirkhan 2:666ad168bacb 233 }
Victor_Mirkhan 2:666ad168bacb 234 }
Victor_Mirkhan 2:666ad168bacb 235 }
Victor_Mirkhan 2:666ad168bacb 236
Victor_Mirkhan 2:666ad168bacb 237 // Esse loop faz com que, caso e enquanto o contador for maior que zero,
Victor_Mirkhan 2:666ad168bacb 238 // o motor proporcione o movimento FOWARD.
Victor_Mirkhan 2:666ad168bacb 239
Victor_Mirkhan 2:666ad168bacb 240 if (count > 0) {
Victor_Mirkhan 2:666ad168bacb 241 while(count > 0) {
Victor_Mirkhan 2:666ad168bacb 242 motor1->run(StepperMotor::FWD);
Victor_Mirkhan 2:666ad168bacb 243 wait(0.1);
Victor_Mirkhan 2:666ad168bacb 244 count = count - 1; //Decrescenta 1 ao contador a cada 0.1seg, aproximando-o de zero.
Victor_Mirkhan 2:666ad168bacb 245 if (count <= 0) { //Se o contador mudar para 0 ou negativo, motor para e quebra o while.
Victor_Mirkhan 2:666ad168bacb 246 motor1->hard_stop();
Victor_Mirkhan 2:666ad168bacb 247 count = 0;
Victor_Mirkhan 2:666ad168bacb 248 break;
Victor_Mirkhan 2:666ad168bacb 249 }
Victor_Mirkhan 2:666ad168bacb 250 }
Victor_Mirkhan 2:666ad168bacb 251 }
Victor_Mirkhan 2:666ad168bacb 252
Victor_Mirkhan 2:666ad168bacb 253 count = 0;
Victor_Mirkhan 2:666ad168bacb 254
Victor_Mirkhan 2:666ad168bacb 255 }
Victor_Mirkhan 2:666ad168bacb 256 }