Projeto de software do projeto mecatrônico
Dependencies: Servo TCS3200 TextLCD2 X_NUCLEO_IHM01A1 mbed
Fork of PROJETO by
Diff: main.cpp
- Revision:
- 2:666ad168bacb
- Parent:
- 1:30696e4d196b
- Child:
- 3:77d4b108568c
diff -r 30696e4d196b -r 666ad168bacb main.cpp --- a/main.cpp Wed Aug 11 09:15:10 2010 +0000 +++ b/main.cpp Fri May 12 18:46:45 2017 +0000 @@ -1,16 +1,256 @@ -#include "QEI.h" +#include "mbed.h" +#include "RPG.h" +#include "L6474.h" + +// Seta o encoder utilizado. Precisa de duas entradas (D14 e D15) e uma terceira +// que seria o seu botão. Como nosso encoder não tem um botão, declara-se qualquer +// pino, já que nem será utilizado. + +RPG rpg1(D14,D15,PC_13); + +/*-----Declaração dos pinos e suas funções-----*/ + +//PINOS DE INTERRUPÇÃO. + +// Pinos D15 e D14 são ligados ao encoder e setados como pinos de interrupção. +InterruptIn encod1(D15); +InterruptIn encod2(D14); + +// Botão do usuario é setado para interrupção, especialmente para a fase de +// prototipação. +InterruptIn usuario(PC_13); + +//PINOS DE SAIDA DIGITAL. + +// Declara o Led da placa como saída digital. +DigitalOut Led(D13); + +/*-----Declaração de variaveis globais do código-----*/ + +bool refer1 = false; // Parametro para o referenciamento do motor 1 +bool refer2 = false; // Parametro para o referenciamento do motor 2 +bool refer3 = false; // Parametro para o referenciamento do motor 3 + +int count; // Contagem de pulsos do encoder +int change = 0; // Numero que varia de 0 a 2, indicando qual set de velocidade será usado +int dirt = 0; // Variavel que pega o estado presente do encoder + +// Criação de uma struct (basicamente, uma classe) para sets de velocidades e acelerações +// diferentes. Cada objeto dessa struct possui os argumentos de velocidades max e min e +// aceleração e desaceleração. + +struct set_velocidades { + int maxspeed; + int minspeed; + int ac; + int dc; +}; + +struct set_velocidades set[3]; //Cria um objeto da struct set_velocidades com três + //objetos dentro dele (basicamente, um objeto triplo). + +//Struct para sets de coordenadas, com argumentos de posição em x,y e z. +struct Coordenadas { + int posx; + int posy; + int posz; +}; + +struct Coordenadas PickPos,DropPos[3]; //Cria objeto unico para posição de pick, e + //objeto triplo para posição de place. + +// Declaração dos motores utilizados. + +L6474 *motor1; +L6474 *motor2; +L6474 *motor3; + +/*-----Declaração de funções auxiliares----*/ + +// Função chamada ao atingir o sensor fim de curso. Para o motor e aciona o zera- +// mento ao alterar o valor da variavel refer. + +void motor1_off(void) { + motor1->hard_stop(); + printf("Interrupcao Feita!\r\n"); + refer1 = true; +} + +// Função chamada na interrupção de um dos sentidos de rotação do encoder. Basicamente, +// desabilita as interrupções enquanto opera ("disable_irq()"), aumenta o contador e +// depois ativa as interrupções novamente ("enable_irq()"). -Serial pc(USBTX, USBRX); -//Use X4 encoding. -//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING); -//Use X2 encoding by default. -QEI wheel (p29, p30, NC, 624); +void mais(void){ + encod2.disable_irq(); + encod1.disable_irq(); + count = count + 1 ; + wait(0.002); + encod1.enable_irq(); + encod2.enable_irq(); +} + +// Função chamada na interrupção de um dos sentidos de rotação do encoder. Basicamente, +// desabilita as interrupções enquanto opera ("disable_irq()"), diminui o contador e +// depois ativa as interrupções novamente ("enable_irq()"). + +void menos(void){ + encod2.disable_irq(); + encod1.disable_irq(); + count = count - 1; + wait(0.002); + encod1.enable_irq(); + encod2.enable_irq(); +} + +// Função que seta para o motor 1 suas velocidades máxima e minima, aceleração +// e desaceleração. + +void set_aceleracoes(int maxspeed,int minspeed,int ac,int dc) { + motor1->set_max_speed(maxspeed); + motor1->set_min_speed(minspeed); + motor1->set_acceleration(ac); + motor1->set_deceleration(dc); +} + +void save_pick(void) { + PickPos.posx = motor1->get_position(); + PickPos.posy = motor2->get_position(); + PickPos.posz = motor3->get_position(); + printf("Posicao de Pick:\r\n"); + printf("X = %d, Y = %d, Z = %d\r\n",PickPos.posx,PickPos.posy,PickPos.posz); +} +//Função que é chamada logo na inicialização do programa para realizar o zera- +// mento das posições dos motores. + +void seta_origem() { + + set_aceleracoes(7000,2000,1000,500); //Seta uma alta aceleração para o referenciamento + usuario.rise(&motor1_off); //Chamada do fim de curso para a função de interrupção + + while(1) { + + //Motor continua andando em uma só direção enquanto variavel refer1 estiver + // em FALSE + + if (refer1 == false){ + motor1->run(StepperMotor::BWD); + motor1->wait_while_active(); + } + + // Se a interrupção for chamada, a variavel refer1 se torna TRUE, acionando + // os comandos a seguir. + + else { + motor1->move(StepperMotor::FWD,1000); //Leve recuo + motor1->wait_while_active(); + motor1->set_home(); // Seta posição de Home + int HomePosition = motor1->get_position(); + printf("Posicao Home = %d\r\n",HomePosition); //Verificar que HomePosition = 0 + refer1 = false; + break; //Quebra do loop while, pois referenciamento do motor foi feito + } + + } +} + +// Função chamada para alternar entre os sets de velocidade. + +void muda_velocidade() { + change = change + 1; + if (change == 3) { + change = 0; + } + printf("Velocidade %d\r\n",change + 1); + set_aceleracoes(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc); +} int main() { + + //Prepara os 3 sets de velocidade, com velocidaes e acelerações variadas. + + set[0].maxspeed = 2000; + set[0].minspeed = 1000; + set[0].ac = 500; + set[0].dc = 500; + + set[1].maxspeed = 4000; + set[1].minspeed = 2000; + set[1].ac = 750; + set[1].dc = 750; + + set[2].maxspeed = 7000; + set[2].minspeed = 5000; + set[2].ac = 1000; + set[2].dc = 1000; + + + DevSPI dev_spi(D11, D12, D13); - while(1){ - wait(0.1); - pc.printf("Pulses is: %i\n", wheel.getPulses()); + //Inicialização dos componentes dos motores + motor1 = new L6474(D2, D8, D7, D9, D10, dev_spi); + motor2 = new L6474(D2, D8, D4, D3, D10, dev_spi); + motor3 = new L6474(D2, D8, D5, D6, D10, dev_spi); + + if (motor1->init() != COMPONENT_OK) { + exit(EXIT_FAILURE); + } + if (motor2->init() != COMPONENT_OK) { + exit(EXIT_FAILURE); + } + if (motor3->init() != COMPONENT_OK) { + exit(EXIT_FAILURE); } - -} + + // Após inicializar tudo, inicia o zeramento. + seta_origem(); + + //Seta a velocidade inicial como a primeira combinação do set + set_aceleracoes(set[0].maxspeed,set[0].minspeed,set[0].ac,set[1].dc); + + while(1) { + + //Declara as funções a serem chamadas nas bordas se subida de cada entrada. + + encod1.rise(&mais); + encod2.rise(&menos); + usuario.rise(&muda_velocidade); + //usuario.rise(&save_pick); + + /*---PAR DE LOOPS PARA O JOG---*/ + + // Esse loop faz com que, caso e enquanto o contador for menor que zero, + // o motor proporcione o movimento BACKWARD. + + if (count < 0) { + while(count < 0) { + motor1->run(StepperMotor::BWD); + wait(0.1); + count = count + 1; //Acrescenta 1 ao contador a cada 0.1seg, aproximando-o de zero. + if (count >= 0) { //Se o contador mudar para 0 ou positivo, motor para e quebra o while. + motor1->hard_stop(); + count = 0; + break; + } + } + } + + // Esse loop faz com que, caso e enquanto o contador for maior que zero, + // o motor proporcione o movimento FOWARD. + + if (count > 0) { + while(count > 0) { + motor1->run(StepperMotor::FWD); + wait(0.1); + count = count - 1; //Decrescenta 1 ao contador a cada 0.1seg, aproximando-o de zero. + if (count <= 0) { //Se o contador mudar para 0 ou negativo, motor para e quebra o while. + motor1->hard_stop(); + count = 0; + break; + } + } + } + + count = 0; + + } +} \ No newline at end of file