Diff: main.cpp

Revision:

0:924ec4a5d21b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 09 18:04:33 2017 +0000
@@ -0,0 +1,635 @@
+
+/* Includes ------------------------------------------------------------------*/
+
+/* mbed specific header files. */
+#include "mbed.h"
+//#include "TCS3200.h"
+
+#include "TextLCD.h"
+
+
+TextLCD lcd(PC_13, PB_7, PA_14, PB_12, PB_2, PB_11, TextLCD::LCD20x4); // rs, e, d4-d7
+
+    //lcd.setBacklight(TextLCD::LightOn);
+    //lcd.printf("Billy gordo");
+
+//#include "TCS3200.h"
+//TCS3200 color(PC_4, PB_13, PB_14, PB_15, PA_15);  //Create a TCS3200 object 
+//            S0    S1    S2     S3     OUT
+
+
+/*------------------------- Implementação LCD --------------------------------*/
+
+//#include "TextLCD.h"
+
+// Host PC Communication channels
+//Serial pc(USBTX, USBRX); // tx, rx
+
+// I2C Communication
+//I2C i2c_lcd(D14,D15); // SDA, SCL
+
+//TextLCD_I2C lcd(&i2c_lcd, 0x7E, TextLCD::LCD20x4);
+
+
+/* Definitions ---------------------------------------------------------------*/
+
+
+
+/* Variables -----------------------------------------------------------------*/
+//InterruptIn Botao_X(D11);
+//InterruptIn Botao_Y(D12);
+//InterruptIn Botao_Z(D13);
+//InterruptIn Botao_Ref(PC_12);
+//InterruptIn Botao_Ref_Go(PC_8);
+//InterruptIn Botao_positivo(PC_2);
+InterruptIn FDCX1(PC_9);
+InterruptIn FDCX2(PB_9);\
+InterruptIn FDCY1(PB_9);
+InterruptIn FDCY2(PB_6);
+InterruptIn FDCZ1(PA_7);
+InterruptIn FDCZ2(PC_7);
+InterruptIn botaox1(PC_3);
+InterruptIn botaoy1(PC_5);
+InterruptIn botaoz1(PC_10);
+InterruptIn botaox2(PA_13);
+InterruptIn botaoy2(PC_2);
+InterruptIn botaoz2(PC_12);
+InterruptIn STOP(PA_11);
+InterruptIn HANDL(PC_8);
+//InterruptIn FDCZ1(PC_2);
+//InterruptIn FDCZ2(PC_2);
+
+//ENTRADAS MOTOR
+DigitalOut Eixo_X_Pulso(D2);
+DigitalOut Eixo_Y_Pulso(D3);
+DigitalOut Eixo_Z_Pulso(D4);
+DigitalOut Eixo_X_Dir(D5);
+DigitalOut Eixo_Y_Dir(D6);
+DigitalOut Eixo_Z_Dir(D7);
+DigitalOut Enable(D8);
+DigitalOut Valv(PB_0);
+DigitalOut LEDSTART(PA_4);
+DigitalOut LEDSTOP(PA_1);
+DigitalOut LEDHANDLE(PA_0);
+
+//ENTRADAS MOTOR
+
+DigitalIn Capac(PC_1);
+DigitalIn START(PC_0);
+
+
+//DigitalIn Botao_Y_positivo(A0);
+//DigitalIn Botao_Y_negativo(A1);
+
+//DigitalIn Botao_Z_positivo(A0);
+//DigitalIn Botao_Z_negativo(A1);
+
+//InterruptIn Botao_FIM_X_MAX(A4);
+//InterruptIn Botao_FIM_X_MIN(A5);
+
+//DigitalIn Botao_FIM_X_MAX(A3);
+//DigitalIn Botao_FIM_X_MIN(A4);
+
+//DigitalIn Botao_FIM_X_MAX(A3);
+//DigitalIn Botao_FIM_X_MIN(A4);
+
+int pos_gar_x = 0;
+int pos_gar_y = 0;
+int pos_gar_z = 0;
+int solta_x1 = 0;
+int solta_x2 = 0;
+int solta_x3 = 0;
+int solta_y1 = 0;
+int solta_y2 = 0;
+int solta_y3 = 0;
+int solta_z1 = 0;
+int solta_z2 = 0;
+int solta_z3 = 0;
+int pos_x1 = 0;
+int pos_x2 = 0;
+int pos_x3 = 0;
+int pos_y1 = 0;
+int pos_y2 = 0;
+int pos_y3 = 0;
+int pos_z1 = 0;
+int pos_z2= 0;
+int pos_z3 = 0;
+int zera = 0;
+int HANDLE = 0;
+int STARTCODE = 0;
+int posmidx = 0;
+int posmidy = 0;
+int posmidz = 0;
+
+//var do motor
+bool dir = false;
+int cont_pulses = 0;
+int cur_pos_x = 0;
+int cur_pos_y = 0;
+int cur_pos_z = 0;
+float velocidade = 0.00032;//de 0.01 a 0.0004
+float velocidade2 = 0.0005;
+bool auto_rot = false;
+//var do motor
+
+
+
+/*------------------------ Funções --------------------------*/
+
+void ligabobina(){
+    Valv = 1;
+    }
+
+void desligabobina(){
+    Valv = 0;
+    }
+/*    
+int rgb() { //--------------------------------------PERFEITO
+    int variavel;
+    long red, green, blue, clear;
+    color.SetMode(TCS3200::SCALE_20);
+    while(x<100){
+        red = color.ReadRed();
+        green = color.ReadGreen();
+        blue = color.ReadBlue();
+        clear = color.ReadClear();
+        //printf("RED: %10d     GREEN: %10d     BLUE: %10d     CLEAR: %10d    \r\n ", red, green, blue, clear);
+        if((red<green)&&(red<blue)){
+            //printf("Color: Red \r\n");
+            variavel=1
+        }
+        if((green<red)&&(green<blue)){
+            //printf("Color: Green \r\n");
+        }
+        if((blue<red)&&(blue<green)){
+            //printf("Color: Blue \r\n");
+        }
+        variavel = 0;
+        wait(0.1);
+        x++;
+        }
+        return variavel;
+}
+*/
+/*
+int rgb(){
+     long red, green, blue, clear;
+     
+     //Set the scaling factor to 100%
+      color.SetMode(TCS3200::SCALE_100);        
+    int bob=0;
+    int cor=0;
+      while(bob<10){
+          //Read the HIGH pulse width in nS for each color.
+          //The lower the value, the more of that color is detected 
+          red = color.ReadRed();
+          green = color.ReadGreen();
+          blue = color.ReadBlue();
+          clear = color.ReadClear();
+ 
+          printf("RED: %10d     GREEN: %10d     BLUE: %10d     CLEAR: %10d     ", red, green, blue, clear);
+             
+          wait(0.1);
+ 
+        if(red>50)
+         {
+             printf("VERMELHO. \r\n"); // Vermelho
+             cor=1;
+         }
+        if(blue>50)
+         {
+             printf("AZUL. \r\n"); // Azul
+             cor=0;
+         }
+                
+        else
+         {
+             printf("VERDE. \r\n"); // Verde
+          }
+          bob++;
+          }
+
+    return cor;
+    }    
+  */  
+  int rgb(){
+      return;
+      }
+      
+int capacitivo(void){
+    int y=0;
+    int C=0;
+    while (y<10){
+    int C= Capac;
+    y++;
+    }
+    return C;
+    }
+
+void direcao(int motor,int posf){
+    dir = true ;
+    switch(motor){
+        case 1:
+        if (posf<cur_pos_x){
+        while(dir == true && cur_pos_x !=posf){
+        Eixo_X_Dir = 0;
+        Eixo_X_Pulso = 1;
+        wait(velocidade);
+        Eixo_X_Pulso = 0;
+                wait(velocidade);
+        cur_pos_x -= 1;
+        //dir == false;
+        }}
+        
+        if (posf>cur_pos_x){
+        while(dir == true && cur_pos_x !=posf){
+        Eixo_X_Dir = 1;
+        Eixo_X_Pulso = 1;
+        wait(velocidade);
+        Eixo_X_Pulso = 0;
+                wait(velocidade);
+        cur_pos_x += 1;
+        //dir == false;
+        }}
+        break;
+            
+        case 2:
+        if (posf<cur_pos_y){
+        while(dir==true && cur_pos_y !=posf){
+        Eixo_Y_Dir = 0;
+        Eixo_Y_Pulso = 1;
+        wait(velocidade);
+        Eixo_Y_Pulso = 0;
+                wait(velocidade);
+        cur_pos_y -= 1;
+        //dir == false;
+          }}
+        
+        if (posf>cur_pos_y){
+        while(dir==true && cur_pos_y !=posf){
+        Eixo_Y_Dir = 1;
+        Eixo_Y_Pulso = 1;
+        wait(velocidade);
+        Eixo_Y_Pulso = 0;
+                wait(velocidade);
+        cur_pos_y += 1;
+        //dir == false;
+        }}
+        
+        break;
+                            
+        case 3:
+        if (posf<cur_pos_z){
+        while(dir == true && cur_pos_z !=posf){
+        Eixo_Z_Dir = 0;
+        Eixo_Z_Pulso = 1;
+                wait(velocidade);
+        Eixo_Z_Pulso = 0;
+                wait(velocidade);
+        cur_pos_z -= 1;
+        //dir == false;
+                }
+        }
+        if (posf>cur_pos_z){
+        while(dir == true && cur_pos_z !=posf){
+        Eixo_Z_Dir = 1;
+        Eixo_Z_Pulso = 1;
+        wait(velocidade);
+        Eixo_Z_Pulso = 0;
+                wait(velocidade);
+        cur_pos_z += 1;
+        //dir == false;
+                }
+        }
+        break;  
+    
+    }       
+    return;
+    }  
+
+
+void direcao_p(int motor){
+    dir = true ;
+    switch(motor){
+        while(dir == true){
+        case 1:
+        Eixo_X_Dir = 1;
+        Eixo_X_Pulso = 1;
+        wait(velocidade);
+        Eixo_X_Pulso = 0;
+                wait(velocidade);
+        cur_pos_x += 1;
+        //dir == false;
+        }
+        break;
+                
+                
+        case 2:
+        while(dir==true){
+        Eixo_Y_Dir = 1;
+        Eixo_Y_Pulso = 1;
+        wait(velocidade);
+        Eixo_Y_Pulso = 0;
+                wait(velocidade);
+        cur_pos_y += 1;
+        //dir == false;
+                }
+        break;
+                
+                
+        case 3:
+        while(dir == true){
+        Eixo_Z_Dir = 1;
+        Eixo_Z_Pulso = 1;
+        wait(velocidade);
+        Eixo_Z_Pulso = 0;
+                wait(velocidade);
+        cur_pos_z += 1;
+        //dir == false;
+                }
+        break;
+                
+                
+    
+    }
+    return;
+    }
+
+void FDCY(){
+    cur_pos_y=0;
+    direcao(2,1500);
+    cur_pos_y=0;
+}    
+
+void motor_off(){
+    dir = false;
+    zera++;
+    }
+
+void direcao_n(int motor){
+    dir = true ;
+    switch(motor){
+        while(dir == true){
+        case 1:
+        Eixo_X_Dir = 0;
+        Eixo_X_Pulso = 1;
+        wait(velocidade);
+        Eixo_X_Pulso = 0;
+                wait(velocidade);
+        cur_pos_x -= 1;
+        //dir == false;
+        }
+        break;
+                
+                
+        case 2:
+        while(dir==true){
+        Eixo_Y_Dir = 0;
+        Eixo_Y_Pulso = 1;
+        wait(velocidade);
+        Eixo_Y_Pulso = 0;
+                wait(velocidade);
+        cur_pos_y -= 1;
+        //dir == false;
+          }
+        break;
+                
+                
+        case 3:
+        while(dir == true){
+        Eixo_Z_Dir = 0;
+        Eixo_Z_Pulso = 1;
+                wait(velocidade);
+        Eixo_Z_Pulso = 0;
+                wait(velocidade);
+        cur_pos_z -= 1;
+        //dir == false;
+                }
+        break;
+            
+    }
+    Eixo_X_Pulso = 0;
+    Eixo_Y_Pulso = 0;
+    Eixo_Z_Pulso = 0;        
+    return;
+    } 
+
+void parax(){
+     direcao_n(1);
+     }
+        
+void paray(){
+     printf("mexeu");
+     direcao_n(2);
+     
+     }
+       
+void paraz(){
+     direcao_p(3);
+     }
+                          
+     
+void zeramentox(){
+    //if (zera==0){
+    printf("ff");
+    lcd.locate(0,0);
+    lcd.printf("Zerando eixo x");
+    direcao_n(1);
+    }
+
+void zeramentoy(){
+    lcd.locate(0,0);
+    lcd.printf("Zerando eixo y");
+    //if (zera==1){
+    direcao_p(2);
+    }
+    
+void zeramentoz(){
+   lcd.locate(0,0);
+   lcd.printf("Zerando eixo z");
+   // if (zera==2){
+    direcao_n(3);
+    }
+
+void handd(){
+    HANDLE=0;
+    LEDHANDLE = 0;
+    }
+    
+void ciclo_off(){
+    dir = false;
+    STARTCODE=0;
+    dir = false;
+    
+    }
+    
+void ciclo(int xp, int yp, int zp){
+    direcao(3,posmidz);
+    STARTCODE = 0;
+    LEDSTART = 1;
+    STOP.rise(&ciclo_off);
+    //direcao(1,posmidx);
+   // direcao(2,posmidy);
+   while (1){
+    if (START ==1){
+        STARTCODE=1;
+        }
+   while (STARTCODE == 1){
+    LEDSTOP = 1;
+    direcao(3,zp);
+    ligabobina();
+    direcao(3,posmidz);
+    int ccor = rgb();
+    int capa = capacitivo();
+    lcd.printf("%d",ccor);
+    wait(3);
+    if (ccor==1 && capa == 1 ){
+        direcao(1,solta_x1);
+        direcao(2,solta_y1);
+        direcao(3,zp+100);
+        desligabobina();
+        direcao(3,posmidz);
+        direcao(1,xp);
+        direcao(2,yp);
+
+        }
+    if (ccor == 2 && capa == 1){
+        direcao(1,solta_x2);
+        direcao(2,solta_y2);
+        direcao(3,zp+100);
+        desligabobina();
+        direcao(3,posmidz);
+        direcao(1,xp);
+        direcao(2,yp);
+        
+        }
+        
+    if (capa == 0){
+        direcao(1,solta_x3);
+        direcao(2,solta_y3);
+        direcao(3,zp+100);
+        desligabobina();
+        direcao(3,posmidz);
+        direcao(1,xp);
+        direcao(2,yp);
+        
+        }
+    }
+    //print start
+    }
+    }
+    
+
+void JOG(){
+    lcd.locate(0,0);
+    lcd.printf("JOG ativado");
+    lcd.locate(0,1);
+    lcd.printf("X = %f",cur_pos_x/400);
+    lcd.locate(0,2);
+    lcd.printf("Y = %f",cur_pos_y/400);
+    lcd.locate(0,3);
+    lcd.printf("Z = %f",cur_pos_z/400);
+    HANDL.rise(&handd);
+    HANDLE = 1;
+    LEDHANDLE = 1;
+    //botaox.rise(&parax);
+    botaox1.fall(&motor_off);
+    botaoy1.fall(&motor_off);
+    botaoz1.fall(&motor_off);
+    botaox2.fall(&motor_off);
+    botaoy2.fall(&motor_off);
+    botaoz2.fall(&motor_off);
+    wait(2);
+    while(1){
+    while(HANDLE==1){
+        lcd.cls();
+        lcd.locate(0,0);
+        lcd.printf("JOG ativado");
+        lcd.locate(0,1);
+        float bobaox=3*cur_pos_x/400;
+        float bobaoz=3*cur_pos_z/400;
+        float bobaoy=3*cur_pos_y/400;
+        lcd.printf("X = %.2f mm",bobaox);
+        lcd.locate(0,2);
+        lcd.printf("Y = %.2f mm",bobaoy);
+        lcd.locate(0,3);
+        lcd.printf("Z = %.2f mm",bobaoz);
+        if (botaox1==1){
+            direcao_n(1);
+            }
+        if (botaoy1 == 1){
+            direcao_n(2);
+            }
+        if (botaoz1 == 1){
+            direcao_n(3);
+            }
+        if (botaox2==1){
+            direcao_p(1);
+            }
+        if (botaoy2 == 1){
+            direcao_p(2);
+            }
+        if (botaoz2 == 1){
+            direcao_p(3);
+            }
+            wait(0.2);
+        }
+    
+    if (HANDLE == 0){
+        lcd.cls();
+        lcd.printf("Pos PEGAR Salva!");
+        pos_gar_x=cur_pos_x;
+        pos_gar_y=cur_pos_y;
+        pos_gar_z=cur_pos_z;
+        wait(2);        
+        ciclo(pos_gar_x,pos_gar_y,pos_gar_z);
+        break;
+        }
+        printf("nada");
+        }}
+            
+
+    
+int main(){
+        //lcd.setBacklight(TextLCD::LightOn);
+        lcd.printf("Zeramento em 5");
+        wait(1);
+        lcd.cls();
+        lcd.printf("Zeramento em 4");
+        wait(1);
+        lcd.cls();
+        lcd.printf("Zeramento em 3");
+        wait(1);
+        lcd.cls();
+        lcd.printf("Zeramento em 2");
+        wait(1);
+        lcd.cls();
+        lcd.printf("Zeramento em 1");
+        wait(1);
+        lcd.cls();
+        lcd.printf("Zerando");
+        FDCY1.rise(&motor_off);
+        FDCX1.rise(&motor_off);
+        FDCZ1.rise(&motor_off);
+        zeramentox();
+        cur_pos_x=0;
+        lcd.locate(0,1);
+        lcd.printf("PosX Salva!");
+        wait(1);
+        zeramentoy();
+        cur_pos_y=0;
+        lcd.locate(0,2);
+        lcd.printf("PosY Salva!");
+        wait(1);
+        zeramentoz();//ok
+        lcd.locate(0,3);
+        lcd.printf("PosZ Salva!");
+        cur_pos_z=0;
+        wait(1);
+        JOG();
+        printf("saiu");
+        //ciclo(pos_gar_x,pos_gar_y,pos_gar_z);
+                
+        }
+        
+        
\ No newline at end of file