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

Dependencies:   Servo TCS3200 X_NUCLEO_IHM01A1 mbed TextLCD2

Fork of QEI_HelloWorld by Aaron Berk

Files at this revision

API Documentation at this revision

Comitter:
Victor_Mirkhan
Date:
Mon Jun 05 21:52:27 2017 +0000
Parent:
3:77d4b108568c
Commit message:
Atualiza??o;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r 77d4b108568c -r 5dd6e95b5706 main.cpp
--- a/main.cpp	Mon May 29 14:47:22 2017 +0000
+++ b/main.cpp	Mon Jun 05 21:52:27 2017 +0000
@@ -23,7 +23,7 @@
 InterruptIn confirma(PC_13); // Botão OK do menu (pino do botão do usuario)
 InterruptIn back_btn(PB_7); // Botão para a função "voltar" do menu
 
-InterruptIn FDC(PC_11); // Fim de curso para o eixo Z
+InterruptIn FDC(PC_5); // Fim de curso para o eixo Z
 InterruptIn FDC2(PC_8); // Fim de curso para o eixo X
 InterruptIn FDC3(PC_6); // Fim de curso para o eixo Y
 
@@ -50,9 +50,6 @@
 signed char ref_cursor = 0; // Numero de referência para a posição do cursor
 unsigned char ref_menu = 0; // Numero para indicar o menu atual
 
-
-float conversao = 3/1600; // A variavel é definida para converter a posição de steps para mm (depende do micro step);
-
 /*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.
@@ -127,13 +124,6 @@
     if (ref_cursor > 3) {
         ref_cursor = 0;
     }
-    
-    /* Opção para o menu com os dois tipos de ciclo. Apenas para impedir que 
-       o usuario coloque o cursor em um campo vazio. */
-       
-    if ((ref_cursor > 1) && (ref_menu == 2)) {
-        ref_cursor = 0;
-    }
 }
 
 void desce_cursor(void)
@@ -142,13 +132,6 @@
     if (ref_cursor < 0) {
         ref_cursor = 3;
     }
-    
-    /* Opção para o menu com os dois tipos de ciclo. Apenas para impedir que 
-       o usuario coloque o cursor em um campo vazio. */
-       
-    if ((ref_cursor < 0) && (ref_menu == 2)) {
-        ref_cursor = 1;
-    }
 }
 
 /* FUNÇÃO "conclusao(char n)":
@@ -196,7 +179,7 @@
     lcd.printf("Z: ");
 }
 
-/*FUNÇÃO "menu_estatico(char n)":
+/*FUNÇÃO "menu_passivo(char n)":
     -> A função apresenta um menu estático diferente de acordo com o valor n recebido.
        Os menus aqui disponiveis são estáticos pois são temporários e não possibilitam
        seleção de funções ou movimento do cursor. 
@@ -204,7 +187,7 @@
        função, da inicialização da maquina e seu estado de operação.
 */
  
-void menu_estatico(char n)
+void menu_passivo(char n)
 {
     switch(n) {
         
@@ -321,10 +304,10 @@
 */
 void set_aceleracoesZ(int maxspeed,int minspeed,int ac,int dc)
 {
-    motorZ->set_max_speed(maxspeed);
-    motorZ->set_min_speed(minspeed);
-    motorZ->set_acceleration(ac);
-    motorZ->set_deceleration(dc);
+    motorZ->set_max_speed(maxspeed)/2;
+    motorZ->set_min_speed(minspeed)/2;
+    motorZ->set_acceleration(ac)/2;
+    motorZ->set_deceleration(dc)/2;
 }
 
 void set_aceleracoesX(int maxspeed,int minspeed,int ac,int dc)
@@ -417,17 +400,14 @@
         green = color.ReadGreen();
         blue = color.ReadBlue();
         clear = color.ReadClear();
-
-        if((red<green)&&(red<blue)) {
-
+        
+        if(green > 30) {
             tagy = 2;
-
         }
-        if((green<red)&&(green<blue)&&(sensor)) {
-
+        if((green <= 30)&&(sensor)) {
             tagy = 0;
         }
-        if((green<red)&&(green<blue)&&(!sensor)) {
+        if((green <= 30) && (!sensor)) {
             
             tagy = 1;
         }
@@ -456,34 +436,36 @@
     return elemento;
 }
 
-/* FUNÇÃO "seta_origem()":
+/* FUNÇÃO "sefta_origem()":
     -> A função realiza o referenciamento completo para os 3 eixos, um de cada vez;
     -> Por ser uma função mais longa, o processo é detalhado dentro da própria função;
 */
 
 void seta_origem()
 {
-    menu_estatico(2);
+    menu_passivo(2);
     
     //Seta uma alta velocidade para o referenciamento
     
-    set_aceleracoesZ(3500,1000,100,100); 
-    set_aceleracoesX(3500,1000,100,100); 
-    set_aceleracoesY(3500,1000,100,100); 
-
+    set_aceleracoesZ(3500/2,1200,100/2,100/2); 
+    set_aceleracoesX(4000,3000,100,100); 
+    set_aceleracoesY(4000,3000,100,100); 
+    
+    InterruptIn confirma(PC_13);
+    confirma.mode(PullUp);
+    
     while(1) {
-
-        /*
-        Motor continua andando em uma só direção enquanto variavel referZ estiver
-        em FALSE 
-        */
+        
+        
+        InterruptIn FDC(PC_5);
+        FDC.fall(&motorZ_off); 
+        FDC.mode(PullUp);
+        
+        confirma.fall(&motorZ_off);
         
         //Chamada do fim de curso para a função de interrupção
         
-        FDC.fall(&motorZ_off); 
-        FDC2.fall(&motorX_off);
-        FDC3.fall(&motorY_off);
-    
+        
         if (referZ == false) {
             motorZ->run(StepperMotor::BWD);
             motorZ->wait_while_active();
@@ -503,16 +485,21 @@
         }
 
     }
-
+    
     while(1) {
-
+            
         // Motor continua andando em uma só direção enquanto variavel referX estiver
         // em FALSE
-
+        
+        InterruptIn FDC(PC_5);
+        FDC.fall(&motorX_off); 
+        FDC.mode(PullUp);
+        
         if (referX == false) {
             motorX->run(StepperMotor::BWD);
             motorX->wait_while_active();
         }
+        
 
         // Se a interrupção for chamada, a variavel referX se torna TRUE, acionando
         // os comandos a seguir.
@@ -522,16 +509,20 @@
             motorX->wait_while_active();
             motorX->set_home(); // Seta posição de Home
             int HomePosition = motorX->get_position();
-            printf("Posicao Home = %d\r\n", HomePosition); //Verificar que HomePosition = 0
             referX = false;
             break;   //Quebra do loop while, pois referenciamento do motor foi feito
         }
 
     }
-
-    //referY = false; //Caso os botôes de fim de curso sejam apertados sem querer antes de seus zeramentos, tem certeza que vai entrar no loop, ja que são interrupçõs
+    
+    referY = false; //Caso os botôes de fim de curso sejam apertados sem querer antes de seus zeramentos, tem certeza que vai entrar no loop, ja que são interrupçõs
+    
     while(1) {
-
+        
+        
+        InterruptIn FDC(PC_5);
+        FDC.fall(&motorY_off); 
+        FDC.mode(PullUp);
         //Motor continua andando em uma só direção enquanto variavel referX estiver
         // em FALSE
 
@@ -573,6 +564,9 @@
     if (change == 3) {
         change = 0;
     }
+    set_aceleracoesZ(set[change].maxspeed/2,set[change].minspeed/2,set[change].ac/2,set[change].dc/2);
+    set_aceleracoesX(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
+    set_aceleracoesY(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
     
     switch(change) {
         case 0: {
@@ -603,10 +597,6 @@
             break;
         }
     }
-               
-    set_aceleracoesZ(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
-    set_aceleracoesX(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
-    set_aceleracoesY(set[change].maxspeed,set[change].minspeed,set[change].ac,set[change].dc);
 
 }
 
@@ -620,7 +610,6 @@
     dirx = true;
     dirz = false;
     diry = false;
-    printf("EIXO X\r\n");
     muda_velocidade();
 }
 
@@ -629,7 +618,6 @@
     dirx = false;
     dirz = false;
     diry = true;
-    printf("EIXO Y\r\n");
     muda_velocidade();
 }
 
@@ -638,7 +626,6 @@
     dirx = false;
     dirz = true;
     diry = false;
-    printf("EIXO Z\r\n");
     muda_velocidade();
 }
 
@@ -779,20 +766,20 @@
     back_btn.fall(&cycle_stop);
     
     if (n == 0) {
-        menu_estatico(3); // Ciclo infinito
+        menu_passivo(3); // Ciclo infinito
     }
     
     if (n == 1) {
-        menu_estatico(4); // Ciclo único
+        menu_passivo(4); // Ciclo único
     }
     
-    unsigned char tag; // Tag que sairá como resultado da função de reconhecimento da peça
+    int tag; // Tag que sairá como resultado da função de reconhecimento da peça
     
     printf("Comeco do ciclo!\r\n");
     
     //Alta velocidade para retornar para a posição de home
     
-    set_aceleracoesZ(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
+    set_aceleracoesZ(set[2].maxspeed/2,set[2].minspeed/2,set[2].ac/2,set[2].dc/2);
     set_aceleracoesX(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
     set_aceleracoesY(set[2].maxspeed,set[2].minspeed,set[2].ac,set[2].dc);
     
@@ -814,6 +801,8 @@
 
     ref_cycle = true;
     
+    garra = 1;
+    
     while(1) {
         
         // Vai para a posição de PICK
@@ -821,22 +810,22 @@
         motorX->wait_while_active();
         motorY->go_to(PickPos.posy);
         motorY->wait_while_active();
-        motorZ->go_to(PickPos.posz);
+        motorZ->go_to(PickPos.posz - 1000);
         motorZ->wait_while_active();
         
         tag = reconhecimento_peca(); // Reconhece a peça e qual posição de Drop irá
-        garra = 1; // Abre a garra
-        wait(1);
         garra = 0.7; // Fecha a garra
         wait(1);
         
         // Vai para a posição de DROP
-        motorZ->go_to(DropPos[tag].posx);
+        motorZ->move(StepperMotor::BWD,3000);
         motorZ->wait_while_active();
-        motorX->go_to(DropPos[tag].posy);
+        motorY->go_to(DropPos[tag].posy);
+        motorY->wait_while_active();
+        motorX->go_to(DropPos[tag].posx);
         motorX->wait_while_active();
-        motorY->go_to(DropPos[tag].posx);
-        motorY->wait_while_active();
+        motorZ->go_to(DropPos[tag].posz);
+        motorZ->wait_while_active();
         wait(1);
         garra = 1; // Garra abre e deixa o objeto
         
@@ -862,6 +851,44 @@
        também;
 */
 
+void paradaZ(void) 
+{
+    motorZ->hard_stop();
+    wait(1);
+    int posicao = motorZ->get_position();
+    if (posicao < 0) {
+        motorZ->move(StepperMotor::FWD,2000);
+    }
+    else {
+        motorZ->move(StepperMotor::BWD,2000);
+    }
+}
+
+void paradaX(void) 
+{
+    motorX->hard_stop();
+    wait(1);
+    int posicao = motorX->get_position();
+    if (posicao < 0) {
+        motorX->move(StepperMotor::FWD,3000);
+    }
+    else {
+        motorX->move(StepperMotor::BWD,3000);
+    }
+}
+void paradaY(void) 
+{
+    motorY->hard_stop();
+    wait(1);
+    int posicao = motorY->get_position();
+    if (posicao < 0) {
+        motorY->move(StepperMotor::FWD,3000);
+    }
+    else {
+        motorY->move(StepperMotor::BWD,3000);
+    }
+}
+
 void jog(char n) {
         menu_jog();
         lcd.locate(3,0);
@@ -913,29 +940,38 @@
     btnX.fall(&ativa_eixoX);
     btnY.fall(&ativa_eixoY);
     btnZ.fall(&ativa_eixoZ);
-    
+
     while(save_pos == false) {
 
         float posz = motorZ->get_position();
         float posx = motorX->get_position();
         float posy = motorY->get_position();
         lcd.locate(3,1);
-        lcd.printf("%.2f mm",posx*conversao);
+        lcd.printf("%.2f mm",posx*3/800);
         lcd.locate(3,2);
-        lcd.printf("%.2f mm",posy*conversao);
+        lcd.printf("%.2f mm",posy*3/800);
         lcd.locate(3,3);
-        lcd.printf("%.2f mm",posz*conversao);
+        lcd.printf("%.2f mm",posz*5/800);
 
         if (jog_pos == true) {
             if (dirx == true) {
+                InterruptIn FDC(PC_5);
+                FDC.mode(PullUp);
+                FDC.fall(&paradaX);
                 motorX->run(StepperMotor::FWD);
                 motorX->wait_while_active();
             }
             if (diry == true) {
+                InterruptIn FDC(PC_5);
+                FDC.mode(PullUp);
+                FDC.fall(&paradaY);
                 motorY->run(StepperMotor::FWD);
                 motorY->wait_while_active();
             }
             if (dirz == true) {
+                InterruptIn FDC(PC_5);
+                FDC.mode(PullUp);
+                FDC.fall(&paradaZ);
                 motorZ->run(StepperMotor::FWD);
                 motorZ->wait_while_active();
             }
@@ -943,14 +979,23 @@
 
         if (jog_neg == true) {
             if (dirx == true) {
+                InterruptIn FDC(PC_5);
+                FDC.mode(PullUp);
+                FDC.fall(&paradaX);
                 motorX->run(StepperMotor::BWD);
                 motorX->wait_while_active();
             }
             if (diry == true) {
+                InterruptIn FDC(PC_5);
+                FDC.mode(PullUp);
+                FDC.fall(&paradaY);
                 motorY->run(StepperMotor::BWD);
                 motorY->wait_while_active();
             }
             if (dirz == true) {
+                InterruptIn FDC(PC_5);
+                FDC.mode(PullUp);
+                FDC.fall(&paradaZ);
                 motorZ->run(StepperMotor::BWD);
                 motorZ->wait_while_active();
             }
@@ -970,10 +1015,9 @@
   
 void back_op(void)
 {
-    if ((ref_menu == 1) || (ref_menu == 2)) {
-        ref_cursor = 4; // O valor de ref_cursor em 4 chama um case que chama a primeira tela dinâmica;
-        enable = true;
-    }
+    ref_cursor = 4; // O valor de ref_cursor em 4 chama um case que chama a primeira tela dinâmica;
+    enable = true;
+    printf("BACK\r\n");
 }
 
 int main()
@@ -1029,7 +1073,7 @@
     lcd.setBacklight(TextLCD::LightOn); // Liga o backlight do LCD
     lcd.setAddress(0,0);
     lcd.setCursor(TextLCD::CurOff_BlkOff); // Desliga o cursor para o menu estático
-    menu_estatico(1);
+    menu_passivo(1);
 
     lcd.setCursor(TextLCD::CurOn_BlkOn); // Liga o cursor novamente pro usuario poder mexe-lo
     menu_dinamico(0);
@@ -1087,12 +1131,19 @@
         btnY.mode(PullUp);
         InterruptIn btnZ(PC_0); // Botão para selecionar o eixo Z
         btnZ.mode(PullUp);
+        
+        InterruptIn back_btn(PB_7);
+        back_btn.mode(PullUp);
+        
         confirma.fall(&selecao_funcao);
+        
         jog_button_pos.fall(&sobe_cursor);
         jog_button_neg.fall(&desce_cursor);
+        
         btnX.fall(&ativa_eixoX);
         btnY.fall(&ativa_eixoY);
         btnZ.fall(&ativa_eixoZ);
+        
         do {
             wait(0.1);
             lcd.locate(0,ref_cursor); // Movimentação do cursor
@@ -1121,6 +1172,7 @@
                         }
                         case 2: {
                             // Operação de referenciamento
+                            //int tag = reconhecimento_peca();
                             seta_origem();
                             flag = false;
                             break;
@@ -1136,7 +1188,9 @@
             }
     
             // Sub-menu para as posições a serem salvas
+            
             if(ref_menu == 1) {
+                back_btn.mode(PullUp);
                 back_btn.fall(&back_op);
                 if (enable == true) {
                     switch(ref_cursor) {
@@ -1144,24 +1198,36 @@
                             // Posição de Pick
                             jog(0);
                             flag = false;
+                            InterruptIn back_btn(PB_7);
+                            back_btn.mode(PullUp);
+                            back_btn.fall(&back_op);
                             break;
                         }
                         case 1: {
                             // Posição de Drop 1
                             jog(1);
                             flag = false;
+                            InterruptIn back_btn(PB_7);
+                            back_btn.mode(PullUp);
+                            back_btn.fall(&back_op);
                             break;
                         }
                         case 2: {
                             // Posição de Drop 2
                             jog(2);
                             flag = false;
+                            InterruptIn back_btn(PB_7);
+                            back_btn.mode(PullUp);
+                            back_btn.fall(&back_op);
                             break;
                         }
                         case 3: {
                             // Posição de Drop 3
                             jog(3);
                             flag = false;
+                            InterruptIn back_btn(PB_7);
+                            back_btn.mode(PullUp);
+                            back_btn.fall(&back_op);
                             break;
                         }
                         case 4: {
@@ -1179,6 +1245,7 @@
             
             if (ref_menu == 2) {
                 back_btn.fall(&back_op);
+                back_btn.mode(PullUp);
                 if (enable == true) {
                     switch(ref_cursor) {
                         case 0: {